ArduCopter Mega: порт на обычную Arduino (тестим)
Переписал код библиотеки ВМР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)
Но на результат это влиять не должно.
tmp_float и так float
Сегодня перепишу. И посчитаю все дроби заранее: “b2* 160*160*/33554432” => “b2* 0,000762939453125”
Я стал (float) прописывать и .000, когда после компиляции местами шел бред и округление переменных.
После работы буду пилить.
В описании Ардуино на arduino.ru указывают, что арифметические операции с плавающей точкой нежелательны. Что то с точностью. Рекомендуют проводить операции сравнения. Это может сказаться на работе такой прошивки?
Это может сказаться на работе такой прошивки?
Совсем не знаю. 30 минут прошивка работает. Мне хочется выжать максимум из бародатчика или искать более точный - мечтаю о “зависании” коптера в режиме alt-hold. Операции с плавающей запятой жрут больше ресурсов, но я попробую, все равно. Надо еще искать, как вывести время цикла.
VMLAB вам в помощь выводите на любую ногу 1 выполняете код, выводите 0 смотрите сколько по времни получилось
float - зло для контроллеров без сопроцессора, округляйте до Nного знака и считайте с фиксированной точкой будет существенно быстрее
“x = 4433000 * (1 - tmp_float)” при такой записи компилятор откинет дробную часть от tmp_float и приведет все к int и long.
float x;
x=1. - tmp_float
Высокую точность можно и с целочисленными операндами получить, если ввести понятие масштаба. Если будет охота заморочиться - пиши, попробую вспомнить чему на дискретке учили 😃
Совсем не знаю. 30 минут прошивка работает. Мне хочется выжать максимум из бародатчика или искать более точный - мечтаю о “зависании” коптера в режиме alt-hold. Операции с плавающей запятой жрут больше ресурсов, но я попробую, все равно. Надо еще искать, как вывести время цикла.
У меня на борту 2 Ардуины. Сейчас поставил второй Мегу 2560. Дуемиланове использую в качестве программатора. Хочу перешить G-OSD. Очень надеюсь на Вашу прошивку. Если перенести баро и сонар на вторые мозги а в основную передавать готовые данные по высоте, как думаете, поможет? Есть второй рабочий барометр, может если усреднять показания с двух, точность может повысится? У меня мечта аналогичная. Только хочется холд на сверхмалых высотах и еще 3 сонара для бокового обзора. Есть в Большом кратере возле Димоны стенка, высотой метров 100. Примерно посередине пещеры. Хочется посмотреть, что там внутри, хотябы возле входа. Если доведу до ума прототип, то окончательная версия будет для дистанционного обследования. Видел как то ролик из штатов, там на акул охотились с помощью коптера. Тоже хочу! Хоть и нет тут серьезных акул, зато возле берега под водой развалины античных городов. Аквалангисты лазят, но они такую площадь не проверят, как коптер может. Если прозрачность воды позволит, конечно. Не Красное море, мутноватая вода. Метров на 5 видимость. Редко 10.
Только хочется холд на сверхмалых высотах и еще 3 сонара для бокового обзора.
В принципе, холд уже работает. Если коптер не наклонять сильно, то все пучком. Я выкладывал видео, как моя страшилка висит в сантиметрах 10-20 от земли. Причем был сильный ветер.
Еще идея прорезалась, снимать показания с сонара только при нулевом левеле, ± сколько нибудь, чтобы совсем уж не перестал работать. Когда луч вертикально примерно. При кренах все равно врет, а при болтанке через ноль проходит. К сожалению программер из меня, как из бутылки молоток, но надеюсь надергать код из интернета, из готовых проектов.
Есть второй рабочий барометр, может если усреднять показания с двух, точность может повысится? У меня мечта аналогичная.
Думаю, 2 шумящих барометра не помогут. Если повезет при инициализации, то один даст дрейф -50см, другой +50см, в итоге суммарный дрейф = 0. Иначе не повезло.
Я присматриваюсь к “MS5611-01 MS5611 Barometric Pressure Sensor Variometer”, пока их нет в наличии на ибей. Заявленная точность 10см, 1Па.
“x = 4433000 * (1 - tmp_float)” при такой записи компилятор откинет дробную часть от tmp_float и приведет все к int и long.
float x;
x=1. - tmp_float
Именно так и было. Весь вечер страдал, разозлился и всем прописал (флоат .0000).
Высокую точность можно и с целочисленными операндами получить, если ввести понятие масштаба
Можно умножать коэффициенты типа (0.000019876) на 100 000 000 и хранить как int32_t. Только немного повозиться с библиотекой.
В sensors.pde отдавать не реальное давление 10130(0) Па, а 101300 0(0) попугаев. И пусть последний разряд прыгает, это будет не 1Па, а сотые доли Паскаля. Все-равно, потом используется соотношение: Pтекущее \ Рground.
И еще часть кода я перенесу из calculation() в init() - рассчет x, y, которые статичны.
В принципе, холд уже работает. Если коптер не наклонять сильно, то все пучком. Я выкладывал видео, как моя страшилка висит в сантиметрах 10-20 от земли. Причем был сильный ветер.
Сылку не кинете? Долго искать по всей ветке. Сейчас вот снял коптера с ног. Приделываю свеже полученные приблуды. ЖПС на хвостовую балку недалеко от платы. Там проходят провода к моторам, но они у меня везде проходят, попробую поиграться, найдет этого ЖПСа или нет. В комплекте с G-OSD пришел. Осд перешивать нужно, то, что есть ни к селу ни к городу.
Еще идея прорезалась, снимать показания с сонара только при нулевом левеле, ± сколько нибудь, чтобы совсем уж не перестал работать.
Так нельзя делать! во всяком случае, пока у вас включен автопилот (ALT_Hold). Т.к. если в какой то момент не снимать показания с сонара, то откуда брать высоту? На барометр нельзя резко переходить, т.к. может оказаться, что коптер висит в 15 см от земли, а баро скажет что 50см… и автопилот благополучно воткнет его в землю.
Если что то и делать, так это ограничивать угол наклона коптера при работе с сонаром. Т.е. на малой высоте не давать наклонять коптер больше 15гр. (Но при этом, возможна ситуация, когда вы не сможете удерживать коптер на одном месте (при сильном ветре))
Брать предыдущее показание. Все равно высота не меняется резко. Обновлять при переходе через ноль
снимать показания с сонара только при нулевом левеле, ± сколько нибудь, чтобы совсем уж не перестал работать.
Смотрите место в ArdupirateMega.pde, где вычисляется высота. Там кусок кода закомментирован, который корректирует высоту по cos\sin наклонов. Переменные наклонов можете использовать для своих целей, их можно вывести в Serial и понаблюдать, какие у них значения. if (sonar_alt < 600) and ( (round(x)>30 degree) OR (round(y) > 30 degree) ) {alt = 0.5*baro_alt + 0.5*sonar_alt; //при сильном наклоне подмешиваем баро_высоту, потому что сонар отваливается }- на словах. Только баро дрейфует на 50 см!
Сейчас в коде на сверхмалой высоте коэффициент_баро -> 0, если пропадает и sonar_alt, то морковка.
Не совсем то. Не учитываются показания сонара вообще при крене. Нужно учитывать постоянно, но обновлять переменную с сонара только вблизи левел = 0
Явно ложные показания будут игнорироваться.
Не совсем то. Не учитываются показания сонара вообще при крене. Нужно учитывать постоянно, но обновлять переменную с сонара только вблизи левел = 0 Ответить с цитированием Ответить с цитированием Поблагодарить автора Спасибо! Кинуть помидором
А надо ли вообще учитывать крен? Там высота будет незначительно изменятся (мне кажется). А вот не учитывать текущую высоту нельзя - т.к. если вы например летите куда то, естественно коптер будет наклонен - а высота то старая будет все время - в итоге краш.
при крене
Только проблема, что при крене коптер теряет высоту, поэтому будет точно не 1 метр. А на сильном крене он сваливается.
Предлагаю при сильном крене и отваливающемся сонаре “подмешивать” baro_alt. Как только уровень выровняется, влияние баро_альт сильно ослабить, и использовать сонар_альт на 98%
level>=30: alt = 0.8* alt_предыдущая + 0.2*baro_alt
level<30: alt = 0.98* sonar_alt + 0.02*baro_alt