ArduCopter Mega: порт на обычную Arduino (тестим)
он пишет ошибку
Попробуй в другой версии планера. И проверь пиды: у меня ругался, но пиды менял
Напоминаю, что для MegaPiratesNG 2.0.40 подходит планер версии 66 (не выше).
Нашел интересные данные, как повысить точность ВМР085 до 1.5 дюйма (во что слабо верится).
Точные данные получить наверное можно, только вот с какой задержкой, мы их получим?
Почему то после установок в Хардвере, хотя данные и запоминаются в таблице (но теряются после перезагрузки, но далеко не всегда) на экране с авиагоризонтом не всегда изменения заметны. Как будто система учитывает только то, что вписано в КЛИ режиме.
Да, я то же заметил этот косячок в планере. Там совершенно неочевидно, когда деклинейшен записывается в коптер, кнопки “Сохранить” там нету. Возможно деклинейшн записывается, когда мы например PIDы меняем или еще при каких то действиях…
Я тут подумал, что наш дешевый сонар, можно применить разве что для автопосадки. При полетах на низкой высоте, велика вероятность оказаться в земле. Особенно в ветреною погоду. Т.к. при относительно небольшом наклоне коптера относительно земли, сонар перестает работать…
Либо как вариант, на низкой высоте, ограничивать максимальный угол наклона коптера…
Товарищи, через APC220 вообще возможно пиды записывать во время работы коптера?
Мне он пишет ошибку Failed set ИМЯ_ПАРАМЕТРА
Да, проверено много раз! И не только ПИД, но и Левел, деклинацию. Ругается, если в качестве разделителя ставить “,”, а не точку: 2.887
Точные данные получить наверное можно, только вот с какой задержкой, мы их получим?
Используется не фильтрация, а калибровочные данные датчика, текущая температура и MSB, LSB и XLSB.
s = T − 25
x = x2*s^2 + x1*s + x0
y = y2*s^2 + y1*s + y0
z = pu − x \ y, где Pu - измеренное давление без корректировки
p2, p1, p0 - коэффициенты из мат.модели датчика.
P = p2*z^2 + p1*z + p0 = истинное давление.
Да, проверено много раз! И не только ПИД, но и Левел, деклинацию. Ругается, если в качестве разделителя ставить “,”, а не точку: 2.887
Не знаю, как именно планер передает значения, но если в текстовом виде, то тогда понятно откуда ноги растут. В зависимости от региональных настроек винды, числа могу вводится с точкой или запятой. А коптер понимает всегда только с точкой (или с запятой, надо проверить). Поэтому у некоторых людей, наблюдаются глюки с планером (если разделитель не совпадают).
Используется не фильтрация, а калибровочные данные датчика, текущая температура и MSB, LSB и XLSB. s = T − 25 x = x2*s^2 + x1*s + x0 y = y2*s^2 + y1*s + y0 z = pu − x \ y, где Pu - измеренное давление без корректировки p2, p1, p0 - коэффициенты из мат.модели датчика. P = p2*z^2 + p1*z + p0 = истинное давление.
если так, то почему сам датчик не вычисляет давление по этим калибровочным данным?
если так, то почему сам датчик не вычисляет давление по этим калибровочным данным?
не знаю точно. Сказано, что калибровочные данные уникальны для каждого датчика (в его EEPROM хранятся коэффициенты смещения, температурной компенсации…всего 176 бит). Он выплевывает калибровки, raw_ температуру и давление. Остальное - сам, ручками. Наверное, в датчике простая логика, которую не нагружают дополнительными вычислениями.
Кстати, желательно обращаться к датчику с частотой не более 40Гц, лучше 20Гц (50 мс). Если oss=3 (максимальное разрешение), то время обработки запроса 25.5 мс.
Нужно или выставить малое oss и самому фильтровать\усреднять, зато иметь частоту опроса до 128Гц. Или использовать высокое разрешение oss=3, но частота опроса падает.
Температура сенсора используется только для корректировки показаний (она влияет на сам сенсор, а не на давление), поэтому не играет роли, где стоит барометр. Еще, чтоб не забыть: “при делении на степени двойки путем сдвига вправо не мультиплицировал знаковый разряд, а просто посылал туда 0. Это приводило к тому, что отрицательные числа при делении на степени двойки становились положительными! Для положительных чисел все работает нормально. Спасибо, ошибка была вызвана скорее невнимательностью: не привел тип unsigned к signed до вычислений, оставив это компилятору (avr-gcc). И самое удивительное, что при комнатной температуре ошибка почти не проявляла себя.”
не знаю точно. Сказано, что калибровочные данные уникальны для каждого датчика (в его EEPROM хранятся коэффициенты смещения, температурной компенсации…всего 176 бит). Он выплевывает калибровки, raw_ температуру и давление. Остальное - сам, ручками. Наверное, в датчике простая логика, которую не нагружают дополнительными вычислениями.
Посмотрел код работы с BMP085, ну так там и так все по формулам с даташита сделано. Т.е. уже используются калибровочные коэффициенты.
Кстати, желательно обращаться к датчику с частотой не более 40Гц, лучше 20Гц (50 мс). Если oss=3 (максимальное разрешение), то время обработки запроса 25.5 мс.
Обновление барометра идет с частотой 10Hz - что вполне достаточно. OSS=3.
Кстати, можно еще подключить вывод готовности барометра к ардуине, тогда можно будет не ждать окончания измерения данных (В АрдуКоптере так и работает)
Кстати, можно еще подключить вывод готовности барометра к ардуине, тогда можно будет не ждать окончания измерения данных (В АрдуКоптере так и работает)
Нужно паять проводок. В нашей библиотеке прерывания по INT просто закомментированы.
Посмотрел код работы с BMP085, ну так там и так все по формулам с даташита сделано. Т.е. уже используются калибровочные коэффициенты.
Да, списано 1-к-1. Но нет пауз 4.5мс между Write и Read. Я нашел работу от 25 апреля 2011, где автор предлагает использовать другую мат.модель. Вечером испытаю.
Обновление барометра идет с частотой 10Hz
Алексей, как вы это узнали? И как можно получить время одного цикла работы Мегапирата? (чтобы вывести serial.print(cycle_time, DEC); )
В нашей библиотеке прерывания по INT просто закомментированы.
Не, там не прерывания. Просто проверяется состояние пина, если там 0, то пропускается текущая итерация, если 1, то считывается текущее значение и запускается следующее измерение.
Кроме того, для оптимизации, температура считывается 1 раз из 4х. Т.е. читается температура, потом 4 раза давление.
Алексей, как вы это узнали? И как можно получить время одного цикла работы Мегапирата? (чтобы вывести serial.print(cycle_time, DEC); )
Легко - в коде написано 😃 (см. функцию update_altitude в ArduCopterMega.pde)
Кроме того, для оптимизации, температура считывается 1 раз из 4х. Т.е. читается температура, потом 4 раза давление.
Еще интересные фильтры в read_pressure и read_temperature, которые сглаживают быстрый сдвиг raw_data. Пошагово тестировал их, пока не понял.
Еще казус был, что АрдуиноIDE показывает список файлов в проекте справа в виде вертикального списка. В нем нет прокрутки, и на экран 22" он не помещается. Я проморгал, что есть файлы ниже read***.pde и перерыл все папки в library. А потом обнаружил sensors.pde в проекте 😁
Еще интересные фильтры в read_pressure и read_temperature
Обычное усреднение показаний.
Кто нибудь в курсе, что такое HIL?
как я понял hardware in loop - типа когда всё отрабатывается, но без выхода на моторы чтоли, толи для симуляции, толи для дебага
как я понял hardware in loop - типа когда всё отрабатывается, но без выхода на моторы чтоли, толи для симуляции, толи для дебага
Да, я уже нашел code.google.com/p/ardupilot-mega/wiki/Xplane
Кстати, как тебе новая стартовая страница Wiki?
как тебе новая стартовая страница Wiki?
зашибись! =)
Да, проверено много раз! И не только ПИД, но и Левел, деклинацию. Ругается, если в качестве разделителя ставить “,”, а не точку
Если ставить ставить “,” поле вообще становится красным, мол параметр не верный… А “Failed set…” выдает и при верных параметрах, тут дело похоже в АРС220, при УСБ соединении такого нет, а при радио линке выскакивает часто, и чем дальше квадрик от тебя тем сложнее записать параметр, у меня обычно пишется раза с 3…
К стати проверял кто нить на 40 прошивке РТЛ? Сегодня пробовал, чтот ведет себя как в режиме лойт, не разворачивается и летит на базу как в 39 прошивке, а начинает пятится назад возвращаясь домой…
Переписал код библиотеки ВМР085 под float - переменные, формулы используются другие, отличные от целочисленных как в даташите. В исходном варианте Press = 101300 кПа, и последний разряд мог прыгать, т.е. 1 кПа. Сейчас давление Press = 101300.568785 хватает с запасом.
Если без фильтров и с целочисленными переменными, то дисперсия = 1118, с целочисленными переменными и фильтрами D=533. С плавающей запятой и фильтрами D=127.
Высота прыгает ± 10, реже ± 20. Но надо допиливать код.
Переписал код библиотеки ВМР085 под float
в плане производительности это как скажется?
в плане производительности это как скажется?
сейчас попробую вывести цикл_тайм и проверить.
тут дело похоже в АРС220, при УСБ соединении такого нет, а при радио линке выскакивает часто
в консоли странные фразы проскакивают что crc не корректный. как будто помехи идут. может быть из-за Wi-Fi такие проблемы со связью APC220?
Как вывести время цикла? Serial.print(fast_loopTimer, DEC); Serial.print(“mc”); показывает 0mc.
Высота прыгает ±10 см, периодически бывают всплески ±20см, потом снова устаканивается. Еще может стабильно уплыть на 40-50 см, коптер подергаешь - все нормализуется. Такое ощущение, что у мембраны гистерезис.
Показания датчика постоянно корректируются по его температуре (см. void APM_BMP085_Class::Calculate())
APM_BMP085.cpp
/*
APM_BMP085.cpp - Arduino Library for BMP085 absolute pressure sensor
*/
extern "C" {
// AVR LibC Includes
#include <inttypes.h>
#include <avr/interrupt.h>
#include "WConstants.h"
}
#include <Wire.h>
#include "APM_BMP085.h"
#define BMP085_ADDRESS 0x77 //(0xEE >> 1)
#define BMP085_EOC 30 // End of conversion pin PC7
// Constructors ////////////////////////////////////////////////////////////////
//APM_BMP085_Class::APM_BMP085_Class()
//{
//}
// Public Methods //////////////////////////////////////////////////////////////
void APM_BMP085_Class::Init(int initialiseWireLib)
{
byte buff[22];
int i = 0;
//pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input
//if( initialiseWireLib != 0 )
Wire.begin();
oss = 3; // Over Sampling setting 3 = High resolution
BMP085_State = 0; // Initial state
// We read the calibration data registers
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xAA);
Wire.endTransmission();
Wire.requestFrom(BMP085_ADDRESS, 22);
();
while(Wire.available()){
buff[i] = Wire.receive(); // receive one byte
i++;
}
ac1 = ((int)buff[0] << 8) | buff[1];
ac2 = ((int)buff[2] << 8) | buff[3];
ac3 = ((int)buff[4] << 8) | buff[5];
ac4 = ((int)buff[6] << 8) | buff[7];
ac5 = ((int)buff[8] << 8) | buff[9];
ac6 = ((int)buff[10] << 8) | buff[11];
b1 = ((int)buff[12] << 8) | buff[13];
b2 = ((int)buff[14] << 8) | buff[15];
mb = ((int)buff[16] << 8) | buff[17];
mc = ((int)buff[18] << 8) | buff[19];
md = ((int)buff[20] << 8) | buff[21];
sp_c3 = (float)((160.000000000000*ac3)/32768.000000000000);
sp_c4 = (float)((ac4/1000.000000000000)/32768.000000000000);
sp_b1 = (float)((160.000000000000*160.000000000000*b1)/1073741824.000000000000);
sp_c5 = (float)((ac5/32768.000000000000)/160.000000000000);
sp_c6 = ac6;
sp_mc = (float)((mc*2048.000000000000)/(160.000000000000*160.000000000000));
sp_md = (float)(md/160.000000000000);
sp_x0 = ac1;
sp_x1 = (float)((160.000000000000*ac2)/8192.000000000000);
sp_x2 = (float)((160.000000000000*160.000000000000*b2)/33554432.000000000000);
sp_y0 = (float)((float)sp_c4*32768.000000000000);
sp_y1 = (float)((float)sp_c4*sp_c3);
sp_y2 = (float)((float)sp_c4*sp_b1);
sp_p0 = (float)((3791.000000000000-8.000000000000)/1600.000000000000);
sp_p1 = (float)(1.000000000000 - (7357.000000000000/1048576.000000000000));
sp_p2 = (float)((3038.000000000000*100.000000000000)/68719476736.000000000000);
//Send a command to read Temp
Command_ReadTemp();
BMP085_State = 1;
}
// Read the sensor. This is a state machine
// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
uint8_t APM_BMP085_Class::Read()
{
uint8_t result = 0;
if (BMP085_State == 1){
//if (digitalRead(BMP085_EOC)){
ReadTemp(); // On state 1 we read temp
BMP085_State++;
Command_ReadPress();
//}
}else{
if (BMP085_State == 5){
//if (digitalRead(BMP085_EOC)){
ReadPress();
Calculate();
BMP085_State = 1; // Start again from state = 1
Command_ReadTemp(); // Read Temp
result = 1; // New pressure reading
//}
}else{
//if (digitalRead(BMP085_EOC)){
ReadPress();
Calculate();
BMP085_State++;
Command_ReadPress();
result = 1; // New pressure reading
//}
}
}
return(result);
}
// Send command to Read Pressure
void APM_BMP085_Class::Command_ReadPress()
{
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF4);
Wire.send(0x34+(oss << 6)); // write_register(0xF4, 0x34+(oversampling_setting << 6));
Wire.endTransmission();
}
// Read Raw Pressure values
void APM_BMP085_Class::ReadPress()
{
byte msb;
byte lsb;
byte xlsb;
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF6);
Wire.endTransmission();
Wire.requestFrom(BMP085_ADDRESS, 3); // read a byte
while(!Wire.available()) { // waiting
}
msb = Wire.receive();
while(!Wire.available()) { // waiting
}
lsb = Wire.receive();
while(!Wire.available()) { // waiting
}
xlsb = Wire.receive();
RawPress = 256*(long)msb + (long)lsb + (long)xlsb/256.00000000;
if(_offset_press == 0){
_offset_press = RawPress;
RawPress = 0;
}else{
RawPress -= _offset_press;
}
// filter
_press_filter[_press_index++] = RawPress;
if(_press_index >= PRESS_FILTER_SIZE)
_press_index = 0;
RawPress = 0;
// sum our filter
for(uint8_t i = 0; i < PRESS_FILTER_SIZE; i++){
RawPress += _press_filter[i];
}
// grab result
RawPress /= PRESS_FILTER_SIZE;
//RawPress >>= 3;
RawPress += _offset_press;
}
// Calculate Temperature and Pressure in real units. C and mbar!!!!!!!!!! 1mbar = 100 Pa
void APM_BMP085_Class::Calculate()
{
float alpha, T, s, z, x, y;
alpha = (float)((float)sp_c5*(RawTemp-(float)sp_c6));
T = (float)((float)alpha + (float)sp_mc/((float)alpha+(float)sp_md));
s = (float)((float)T - 25);
x = (float)((float)sp_x2*(float)s*(float)s + (float)sp_x1*(float)s + (float)sp_x0);
y = (float)((float)sp_y2*(float)s*(float)s + (float)sp_y1*(float)s + (float)sp_y0);
z = (float)(((uint32_t)RawPress - (float)x) / (float)y);
Press = (float)((float)sp_p2*(float)z*(float)z + (float)sp_p1*(float)z + (float)sp_p0)*100.0000;
Temp = (float)T*10; //так требует софт
}
void APM_BMP085_Class::Command_ReadTemp()
{
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF4);
Wire.send(0x2E);
Wire.endTransmission();
}
// Read Raw Temperature values
void APM_BMP085_Class::ReadTemp()
{
byte tmp;
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF6);
Wire.endTransmission();
Wire.beginTransmission(BMP085_ADDRESS);
Wire.requestFrom(BMP085_ADDRESS,2);
while(!Wire.available()); // wait
RawTemp = Wire.receive();
while(!Wire.available()); // wait
tmp = Wire.receive();
RawTemp = RawTemp*256.000 + tmp;
if(_offset_temp == 0){
_offset_temp = RawTemp;
RawTemp = 0;
}else{
RawTemp -= _offset_temp;
}
// filter
_temp_filter[_temp_index++] = RawTemp;
if(_temp_index >= TEMP_FILTER_SIZE)
_temp_index = 0;
RawTemp = 0;
// sum our filter
for(uint8_t i = 0; i < TEMP_FILTER_SIZE; i++){
RawTemp += _temp_filter[i];
}
// grab result
RawTemp /= TEMP_FILTER_SIZE;
//RawTemp >>= 4;
RawTemp += _offset_temp;
}
// Constructors ////////////////////////////////////////////////////////////////
APM_BMP085_HIL_Class::APM_BMP085_HIL_Class()
{
}
// Public Methods //////////////////////////////////////////////////////////////
void APM_BMP085_HIL_Class::Init(int initialiseWireLib)
{
BMP085_State=1;
}
// Read the sensor. This is a state machine
// We read one time Temperature (state = 1) and then 4 times Pressure (states 2-5)
uint8_t APM_BMP085_HIL_Class::Read()
{
uint8_t result = 0;
if (BMP085_State == 1){
BMP085_State++;
}else{
if (BMP085_State == 5){
BMP085_State = 1; // Start again from state = 1
result = 1; // New pressure reading
}else{
BMP085_State++;
result = 1; // New pressure reading
}
}
return(result);
}
void APM_BMP085_HIL_Class::setHIL(float _Temp, float _Press)
{
// TODO: map floats to raw
Temp = _Temp;
Press = _Press;
}
APM_BMP085.h
#ifndef APM_BMP085_h
#define APM_BMP085_h
#define TEMP_FILTER_SIZE 8
#define PRESS_FILTER_SIZE 8
class APM_BMP085_Class
{
public:
APM_BMP085_Class():
_temp_index(0),
_press_index(0){}; // Constructor
float RawPress;
float RawTemp;
float Temp;
float Press;
//int Altitude;
uint8_t oss;
//int32_t Press0; // Pressure at sea level
void Init(int initialiseWireLib = 1);
uint8_t Read();
float sp_c3, sp_c4, sp_b1, sp_c5, sp_c6, sp_mc, sp_md;
float sp_x0, sp_x1, sp_x2, sp_y0, sp_y1, sp_y2, sp_p0, sp_p1, sp_p2;
int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
uint16_t ac4, ac5, ac6;
private:
// State machine
uint8_t BMP085_State;
// Internal calibration registers
//int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
//uint16_t ac4, ac5, ac6;
//float sp_c3, sp_c4, sp_b1, sp_c5, sp_c6, sp_mc, sp_md;
//float sp_x0, sp_x1, sp_x2, sp_y0, sp_y1, sp_y2, sp_p0, sp_p1, sp_p2;
float _temp_filter[TEMP_FILTER_SIZE];
float _press_filter[PRESS_FILTER_SIZE];
float _offset_press;
float _offset_temp;
uint8_t _temp_index;
uint8_t _press_index;
void Command_ReadPress();
void Command_ReadTemp();
void ReadPress();
void ReadTemp();
void Calculate();
};
class APM_BMP085_HIL_Class
{
private:
uint8_t BMP085_State;
public:
float RawPress;
float RawTemp;
float Temp;
float Press;
//int Altitude;
uint8_t oss;
APM_BMP085_HIL_Class(); // Constructor
void Init(int initialiseWireLib = 1);
uint8_t Read();
void setHIL(float Temp, float Press);
};
#endif
sensors.pde
static void init_barometer(void)
{
#if HIL_MODE == HIL_MODE_SENSORS
hil.update(); // look for inbound hil packets for initialization
#endif
ground_pressure = 0;
ground_temperature = 0;
abs_pressure = 0;
int i;
// We take some readings...
for(i = 0; i < 60; i++){
delay(50);
// get new data from absolute pressure sensor
barometer.Read();
("init %ld, %d, -, %ld, %ld\n", barometer.RawTemp, barometer.Temp, barometer.RawPress, barometer.Press);
}
for(i = 0; i < 20; i++){
delay(50);
#if HIL_MODE == HIL_MODE_SENSORS
hil.update(); // look for inbound hil packets
#endif
// Get initial data from absolute pressure sensor
barometer.Read();
ground_pressure += barometer.Press;
("init %ld, %d, -, %ld, %ld, -, %d, %ld\n", barometer.RawTemp, barometer.Temp, barometer.RawPress, barometer.Press, ground_temperature, ground_pressure);
}
ground_temperature = barometer.Temp;
ground_pressure /= 20.000;
abs_pressure = ground_pressure;
("init %ld\n", abs_pressure);
//SendDebugln("barometer calibration complete.");
}
static long read_barometer(void)
{
float x, tmp_float;
barometer.Read();
abs_pressure = (float)abs_pressure * .6 + (float)barometer.Press *.4; // берем 0.5 от прошлого давления и 0.5 от нового, чтобы не прыгало сильно
tmp_float = (float)abs_pressure /(float)ground_pressure; //формула из авиации: давление на уровне моря заменил на ground_pressure
tmp_float = pow((float)tmp_float, 0.190295);
x = 4433000.000 * (1.000 - (float)tmp_float); // мы узнали высоту относительно текущей земли! потому что использовали abs_pressure \ ground_pressure
return (x);
}
ArducopterMega.pde
// Barometer Sensor variables
// --------------------------
static float abs_pressure;
static float ground_pressure;
static float ground_temperature;
Просьба: в коде нужны нули после запятой? Местами, если не ставить, то компилятор считает формулу как int. Аналично насчет (float) перед переменными - стоит ли явно указывать компилятору везде на это.
В общем, помидорами не бросайте, укажите, где г…код.
Просьба: в коде нужны нули после запятой? Местами, если не ставить, то компилятор считает формулу как int. Аналично насчет (float) перед переменными - стоит ли явно указывать компилятору везде на это.
В общем, помидорами не бросайте, укажите, где г…код.
Это полностью на совести компилятора. В c++ как-то так: www.cyberguru.ru/…/cpp-velvet-way-page78.html
Т.е. в каждой операции операнды приводятся к типу наибольшего.
(float)tmp_float - это зачем? tmp_float и так float.
x = 4433000.000 * (1.000 - (float)tmp_float) ==> x = 4433000 * (1 - tmp_float)
Но на результат это влиять не должно.