Я любитель, перед которым стоял вопрос: как имея в наличии ардуино нано и датчик GY511 собрать инклинометр ( Инклинометр (от лат. incline — наклоняю и …метр) — прибор, предназначенный для измерения угла наклона различных объектов относительно гравитационного поля Земли. Помимо собственно величины угла наклона, может измеряться его направление — азимут).
1. Проблема состояла в том, что я не имел понятия, как вообще работает ардуино и с какой стороны подойти, но все сомнения отпали, когда увидел как там интуитивно просто сориентироваться. Конечно, это не повод не учить этот язык программирования и саму платформу, но смелости прибавилось.
2. Проблема состояла в математике этого устройства. Информации об инклинометрах много, в основном допотопная и "многобуквенная". Пугало не количество букв, а количество незнакомых слов, что они составляли. Образования у меня нет, кроме школьного, поэтому, понять, о чем в этих книгах говорили умные мужи, было сложно, но благодаря интернету я немного прообгрейтился.
Я имел опыт ремонта аналоговых инклинометров, когда в роле датчиков использовалась буссоль (Геодезический инструмент для измерения углов при съёмках на местности, специальный вид компаса. Имеет визирное приспособление. Шкала буссоли часто бывает направлена против часовой стрелки («обратная», или буссольная шкала), что облегчает прямое, без вычислений, взятие магнитных азимутов), но сердце желало большего. На просторах интернета нашел датчики разные, остановился на GY511 (простите, ссылки вставлять не умею, поэтому не буду) - это датчик с акселерометром и магнитометром. Магнитометр нужен для определения азимута, а акселерометр для определения угла наклона относительно одной из осей.
перед тем как выкладывать код я опишу как вычислил математику и где её искал.
1. не надо искать в интернете конкретно «инклинометр», если вы уже работали с подобной техникой и в курсе что это такое, то нет необходимости вбивать в гугле "инклинометр". Просто потому что выйдут потенты выдающихся (это не сарказм) людей, которые проделали титаническую работы по созданию этих восхитительных приборов, и книги об инклинометрах написанные ими же, где описываются принцип работы и тригонометрический лес без пояснений с какого курса математики или геометрии это взято. Думая как это все устроено до меня "допёрло", что в сути мы работаем с векторами(для кого-то это очевидно, а для меня было открытие) и надо искать инфу не об инклинометрах и их расчетов углов, а про вектора и расчетом углов векторов относительно той или иной плоскости.
http://www.pm298.ru/reshenie/vektor.php на этом сайте я прочел об векторной алгебре, а на
http://radiokot.ru/konkursCatDay2014/25/ этом сайте об человеке который подсказал... короче просто тоже, что то подчеркнул у него. в целом чтобы понять как это всё вычитывается, прочтите это всё и вам станет всё ясно. суть в чем, для измерения зенитного угла(
https://ru.wikipedia.org/wiki/%D0%A1%D1 ... 0%B0%D1%82) мы задействуем только акселерометр, но вот чтобы теперь определить направление этого зенитного угла(дальше просто азимут наклона), нужно как-то его привязать к азимуту. У нас теперь есть 3 величины которые необходимо узнать : 1 зенит, 2 азимут наклона, 3 азимут (это то в какую сторону смотрит корпус датчика относительно севера).
1.1. для выяснения зенитного угла использовал данные с акселерометра и рассчитал их по формуле θ(theta)=arccos(Z/sqrt(sq(X)+sq(Y)+sq(Z))). уже проверенная формула работает отлично. почему именно она и почему всё стоит так как тут написано описывается в статьях что я приложил, прочтите и поймёте.
1.2. Для выяснения угла азимута наклона также использовались данные с акселерометра. тут поинтереснее: дело в том, что по тихонько круча(именно так) сам датчик, можно определить какая из осей "привязана" в северу. вращая его я определил, что ось Y в отрицательных значениях(когда смотрит жопой на север), азимут показывает 0. итак если не затрагивать ось Z то мы знаем, что есть на оси X, -X, Y, -Y смотреть сверху, они рисуют нам розу ветров, и я определил что ось -Y ориентирована на север. Когда она отходит от севера... короче почитайте про компасы еще. теперь для определения угла наклона надо узнать в какую сторону наклонился корпус нашего датчика относительно оси -Y и сложить полученный угол с азимутом, то эта сумма и есть реальный азимут наклона. Для определения угла наклона относительно оси –Y используется формула ϕ=arctang(X/Y). Дело в том, что вектор, который происходит во время наклона датчика относительно оси Z, рисует проекцию на плоскость образуемую осями X и Y. мы этой формулой вычисляем угол, этого вектора, относительно оси –Y. Есть еще кое-что: проекция может находиться в разных квадрантах, которые определяются знаками значений X и Y. В статьях это написано. Дальше в коде я это поясню, но суть вы должны были уловить.
1.3. Азимут датчик GY511 посылал на ардуино по I2C. В примерах этого датчика готовые скетчи, я воспользовался ими. Так что я не замарачивался.
2. теперь... имея формулы для вычисления, надо было как-то заставить ардуино посчитать всё. Повозившись на форумах, я понял, что надо учиться. Начиная с моргания лампочек, я дошёл до того, что на мониторе порта (Serial) мне выдавались значения магнитометра и акселерометра. Привязав их к переменным, я получил возможность оперировать ими. На форуме я прочел, как можно высчитывать тангенсы и арктангенсы. Кстати они высчитываются в радианах, в интернете прочел, как радианы перевести в градусы. Написал код и начал проверять. Выводил значения акселерометра X Y Z для того, чтобы проверить, как считает ардуино, и как считаю я на бумажке. В итоге всё получилось, что не могло не радовать. Вот собственно и сам код:
Код:
// программа инклинометрии
// ноги датчика ардуино
// VCC 3.3V
// GND GND
// SCL A5
// SDA A4
// ноги дисплея ардуино
// VSS GND
// VDD 5V
// V0 через резистор на GND
// RS D12
// RW GND
// E D11
// D4 D5
// D5 D4
// D6 D3
// D7 D2
// A 5V
// K GND
//===============
// подключение библиотек
#include <Wire.h> // библиотека по которой общается датчик с ардуиной
#include <LSM303.h> // библиотека самого датчика
#include <LiquidCrystal.h> // а это библиотека для вывода значений на дисплейчик 1602A
//===============
LSM303 compass;
char report[80];
//===============
// переменные, с которыми будем работать
float ax; // переменная хранящие данные оси X акселерометра
float ay; // переменная хранящие данные оси Y акселерометра
float az; // переменная хранящие данные оси Z акселерометра
float m; // переменная хранящие данные азимута (сторона, куда повернут корпус датчика)
float b1; // переменная хранящие данные угла наклона от 0 до 180 градусов. погрешность 0.05 градуса
float b2; // переменная хранящая данные угла поворота наклона относительно оси X акселерометра.
float b3; // переменная хранящая сумму азимута (сторона, куда повернут корпус прибора) и угла поворота наклона относительно оси X акселерометра.
float b4; // переменная хранящая данные азимута наклона
//==============
//==============
//ноги дисплея
const int rs = 12, en = 11, d4 = 5, d5 = 4, d6 = 3, d7 = 2;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
//==============
void setup()
{
Serial.begin(9600);
Wire.begin();
lcd.begin(16, 2);
lcd.print("inklinometr");
delay(2000);
compass.init();
compass.enableDefault();
compass.m_min = (LSM303::vector<int16_t>){-606, -725, -369}; // данные калибровки осей X Y Z
compass.m_max = (LSM303::vector<int16_t>){+614, +529, +711}; // данные калибровки осей X Y Z
}
void loop()
{
compass.read();
float heading = compass.heading();
snprintf(report, sizeof(report), "A: %6d %6d %6d M: %6d %6d %6d",
compass.a.x, compass.a.y, compass.a.z,
compass.m.x, compass.m.y, compass.m.z);
//===========
//присваиваем переменным значения акселерометра
float ax = compass.a.x;
float ay = compass.a.y;
float az = compass.a.z;
float m = heading; // это собственно азимут который определяет на сколько градусов повернут корпус датчика
//==========
//==========
// расчет в первом полушарие полушарии от 0 до 180 градусов
//==========
if (ax > 0) //если значения оси X больше ноля
{
//================
// расчет в первом квадранте от 0 до 90 градусов это если +X +Y
//================
if (ay > 0) //если хначения оси Y больше ноля
{
float b1 = ((acos(az / sqrt(sq(ax) + sq(ay) + sq(az)))) * 180)/3.14; // расчет зенитного угла и превращение его из радианов в градусы
float b2 = (((atan((sqrt(sq(ax)))/sqrt(sq(ay)))))*180)/3.14; // расчет угла проекции вектора относительно оси Y. переменные возводятся в квадрат, а потом выводятся из него для выведения их модуля
float b3 = b2 + m; // здесь идет сложения углов для определения в какую сторону наклонен датчик(азимут наклона)
if (b3 > 359) // если сумма больше 360 градусов
{
float b4 = b3 - 359; // то просто от этой суммы вычитывается 360 и выводится на дисплей
Serial.print("azimut naklona4 = ");
Serial.println(b4);
lcd.setCursor(0, 0);
lcd.print("an");
lcd.print(b4);
}
else // а если нет, то выводится на дисплей как есть
{
float b4 = b3;
Serial.print("azimut naklona4 = ");
Serial.println(b4);
lcd.setCursor(0, 0);
lcd.print("an");
lcd.print(b4);
}
// выводится посчитанный зенитный угол
Serial.print("ugol naklona = ");
Serial.println(b1);
lcd.setCursor(0, 2);
lcd.print("ug.n=");
lcd.print(b1);
}
// конец расчета в первом квадранте от 0 до 90 градусов это если +X +Y
//========
//=====================
// расчет во втором квадранте от 90 до 180 градусов это если +X -Y
//=====================
else //если значения оси Y меньше ноля
{
float b1 = ((acos(az / sqrt(sq(ax) + sq(ay) + sq(az)))) * 180)/3.14; // расчет зенитного угла и превращение его из радианов в градусы
float b2 = ((((atan((sqrt(sq(ay)))/sqrt(sq(ax)))))*180)/3.14)+90; // расчет угла проекции вектора относительно оси X, т.к. мы уже во втором квадранте и прибавляем к полученным данным 90
float b3 = b2 + m; // здесь идет сложения углов, для определения в какую сторону наклонен датчик
if (b3 > 359) // если сумма больше 360 градусов
{
float b4 = b3 - 359; // то просто от этой суммы вычитывается 360 и выводится на дисплей
Serial.print("azimut naklona4 = ");
Serial.println(b4);
lcd.setCursor(0, 0);
lcd.print("an");
lcd.print(b4);
}
else // а если нет, то выводится на дисплей как есть
{
float b4 = b3;
Serial.print("azimut naklona4 = ");
Serial.println(b4);
lcd.setCursor(0, 0);
lcd.print("an");
lcd.print(b4);
}
// выводится посчитанный зенитный угол
Serial.print("ugol naklona = ");
Serial.println(b1);
lcd.setCursor(0, 2);
lcd.print("ug.n=");
lcd.print(b1);
}
// конец расчета во втором квадранте от 90 до 180 градусов это если +X -Y
//========
}
// конец расчета в первом полушарии от 0 до 180 градусов
//==========
//==========
// расчет во втором полушарие полушарии от 180 до 360 градусов
//==========
if (ax < 0) //если значения оси X меньше ноля
{
//===================
// расчет в третьем квадранте от 180 до 270 градусов это если -X -Y
//===================
if (ay < 0) //если значения оси Y меньше ноля
{
float b1 = ((acos(az / sqrt(sq(ax) + sq(ay) + sq(az)))) * 180)/3.14; // расчет зенитного угла и превращение его из радианов в градусы
float b2 = ((((atan((sqrt(sq(ax)))/sqrt(sq(ay)))))*180)/3.14)+180; // расчет угла проекции вектора относительно оси X, т.к. мы уже в третьем квадранте и прибавляем к полученным данным 180
float b3 = b2 + m; // здесь идет сложения углов, для определения в какую сторону наклонен датчик
if (b3 > 359) // если сумма больше 360 градусов
{
float b4 = b3 - 359; // то просто от этой суммы вычитывается 360 и выводится на дисплей
Serial.print("azimut naklona4 = ");
Serial.println(b4);
lcd.setCursor(0, 0);
lcd.print("an");
lcd.print(b4);
}
else // а если нет, то выводится на дисплей как есть
{
float b4 = b3;
Serial.print("azimut naklona4 = ");
Serial.println(b4);
lcd.setCursor(0, 0);
lcd.print("an");
lcd.print(b4);
}
// выводится посчитанный зенитный угол
Serial.print("ugol naklona = ");
Serial.println(b1);
lcd.setCursor(0, 2);
lcd.print("ug.n=");
lcd.print(b1);
}
// конец расчета в третьем квадранте от 180 до 270 градусов это если -X -Y
//========
//=====================
// расчет в четвёртом квадранте от 270 до 360 градусов это если -X +Y
//=====================
else //если значения оси Y больше ноля
{
float b1 = ((acos(az / sqrt(sq(ax) + sq(ay) + sq(az)))) * 180)/3.14; // расчет зенитного угла и превращение его из радианов в градусы
float b2 = ((((atan((sqrt(sq(ay)))/sqrt(sq(ax)))))*180)/3.14)+270; // расчет угла проекции вектора относительно оси X, т.к. мы уже в третьем квадранте и прибавляем к полученным данным 270
float b3 = b2 + m; // здесь идет сложения углов, для определения в какую сторону наклонен датчик
if (b3 > 359) // если сумма больше 360 градусов
{
float b4 = b3 - 359; // то просто от этой суммы вычитывается 360 и выводится на дисплей
Serial.print("azimut naklona4 = ");
Serial.println(b4);
lcd.setCursor(0, 0);
lcd.print("an=");
lcd.print(b4);
}
else // а если нет, то выводится на дисплей как есть
{
float b4 = b3;
Serial.print("azimut naklona4 = ");
Serial.println(b4);
lcd.setCursor(0, 0);
lcd.print("an");
lcd.print(b4);
}
// выводится посчитанный зенитный угол
Serial.print("ugol naklona = ");
Serial.println(b1);
lcd.setCursor(0, 2);
lcd.print("ug.n=");
lcd.print(b1);
}
// конец расчета в четвертом квадранте от 270 до 360 градусов это если -X +Y
//========
}
// здесь выводим азимут, то в какую сторону повёрнут корпус
Serial.print("azimut = ");
Serial.println(heading);
Serial.println(report);
lcd.setCursor(9, 0);
lcd.print("ak");
lcd.print(m);
}
Вот в целом и всё! )) всем удачи в программировании. Пишите, будет интересно поработать вместе.
Антон Цаголов
Atsagolov@inbox.ru