Search in topic

ArduCopter Mega: порт на обычную Arduino (тестим)

не знаю точно. Сказано, что калибровочные данные уникальны для каждого датчика (в его 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)
Но на результат это влиять не должно.

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

Высокую точность можно и с целочисленными операндами получить, если ввести понятие масштаба. Если будет охота заморочиться - пиши, попробую вспомнить чему на дискретке учили 😃