void setup(void)
{
// Get particulars for board
i2cInit(I2CDEV_2);
// Not sure why the ms5611 needs this, but without this line it doesn't work
i2cWrite(0,0,0);
// attempt to initialize barometer
available = ms5611_init();
}
//////////////////// MAIN /////////////////////////////
int main (void) {
int i;
float temperature, pressure;
float QNHPA;
const int QNH_Calib=2; //AO: TODO Make this settable at startup and
//write to nvRAM.
float altitude_m,altitude_ft;
struct ms5611_vars baro;
//Coordinates for writing values on screen:
uint8_t ALTFT_x=190, ALTFT_y=100;
uint8_t QNH_x=90,QNH_y=120; //Location on screen to print QNH
uint8_t ALTM_x=QNH_x, ALTM_y=140;
uint8_t BARO_x=QNH_x,BARO_y=160; //Location on screen to print BARO
uint8_t TEMP_x=QNH_x,TEMP_y=180; //Location on screen to print TEMP
//TODO: Remove obsolete code.
//TODO: Baro_Delay_Max becomes a #define.
// int GPIO_Delay_Max =250; //Led's will stay on for this many ms.
// int BARO_Delay_Max =200; //Baro refresh rate.
//Display related:
int a;
// Both single speed and double speed access works.
// Single speed is slower, needs less memory, vice versa for double speed.
//u8g_InitComFn(&u8g, &u8g_dev_ssd1306_128x64_i2c, u8g_com_hw_i2c_fn);
u8g_InitComFn(&u8g, &u8g_dev_ssd1306_128x64_2x_i2c, u8g_com_hw_i2c_fn);
// Initialize hardware:
disable_JTAG(); //So that some pins (notably LED) are freed up for use.
init_BKP(); //Battery backup/RTC module init.
init_ENC(); //Initialize ports connected to encoder.
init_LED_GPIO(); //Initialize ports connected to LED.
/* Init Chan's Embedded String Functions (xprintf etc) */
xdev_out(uart_putc);
xdev_in(uart_getc);
init_USART1();
// Initialize USART1:
uart_open (USART1, 115200, 0); //USART2 is not supported.
if (SysTick_Config(SystemCoreClock/1000))
while (1);
// Every 1 msec, the timer will trigger a call to the SysTick_Handler.
xprintf ("STM32F103 Naze32/Flip32.\n\r");
xprintf("System core clock rate is %d Hz\n\r",SystemCoreClock);
xprintf("QNH calibration value set to: %d. \n\r",QNH_Calib);
//Turn off LED
LED_OFF();
QNH = BKP_ReadBackupRegister (BKP_DR1);
QNH=1013; //TODO: Naze32 has no provision for backup battery connection.
// The pin V_bat is connected to supply...
// The function works correctly. Needs a board with V_bat wired correctly.
if (QNH<950) //Quick sanity check.
QNH=950;
else if (QNH>1050)
QNH=1050;
ms5611_init(&baro);
ms5611_measure(&baro);
ms5611_calculate(&baro);
QNHPA = (QNH+QNH_Calib)*100; //Convert to Pa. baro.pressure is also in pa.
//Note: Uncomment following line for AGL measurement.
//QNHPA= baro.pressure;
BARO_Delay = BARO_Delay_Max; //Counts the number of timer ticks so far.
//Display an introductory info message and wait 3s before starting:
u8g_FirstPage(&u8g);
do {
draw_Intro();
} while ( u8g_NextPage(&u8g) );
i=0;
while(i < INTRO_WAIT_MS){ //Keep info screen up for some time.
if (TimerEventFlag==TRUE){
TimerEventFlag=FALSE;
++i;
}
}
while (1) {
if (TimerEventFlag==TRUE){
// QNH adjustment used to live here but because of the long delay
// in calculating the altitude, it was moved into the ISR.
TimerEventFlag=FALSE;
}
//Time to update barometer?:
if (BARO_Delay >=BARO_Delay_Max){
BARO_Delay=0;
//LED_ON(); //To check utilization.
ms5611_measure(&baro);
ms5611_calculate(&baro);
//Note: Comment following line for AGL measurement.
QNHPA = (QNH+QNH_Calib)*100;//Convert to Pa. baro.pressure is also in pa.
altitude_m = 44330*(1- powf((baro.pressure/QNHPA),(0.19029495))); //m
altitude_ft =altitude_m *3.28084; //ft.
} //END: if (BARO_Delay >=BARO_Delay_Max)
if (printQNH==TRUE){ //Time to print QNH:
printQNH=FALSE;
u8g_FirstPage(&u8g);
do {
draw_qnh((int)baro.pressure/100,(int)baro.temperature/100,(int)altitude_m,(int)altitude_ft,(int)QNH);
} while ( u8g_NextPage(&u8g) );
BKP_WriteBackupRegister (BKP_DR1, QNH);
} else if (printINFO==TRUE){
u8g_FirstPage(&u8g);
do {
draw_Intro();
} while ( u8g_NextPage(&u8g) );
}else{
u8g_FirstPage(&u8g);
do {
draw_alt((int)baro.pressure/100,(int)baro.temperature/100,(int)altitude_m,(int)altitude_ft,(int)QNH);
} while ( u8g_NextPage(&u8g) );
}
//LED_OFF(); //To check utilization.
} //END: while(1)
} //END: main
int main(void)
{
i2c_bus_t bus;
int ret = i2c_bus_open(&bus, "/dev/i2c-3");
if (ret < 0)
{
fatal("could not open i2c bus", ret);
return EXIT_FAILURE;
}
/* ITG: */
itg3200_dev_t itg;
itg_again:
ret = itg3200_init(&itg, &bus, ITG3200_DLPF_42HZ);
if (ret < 0)
{
fatal("could not inizialize ITG3200", ret);
if (ret == -EAGAIN)
{
goto itg_again;
}
return EXIT_FAILURE;
}
/* BMA: */
bma180_dev_t bma;
bma180_init(&bma, &bus, BMA180_RANGE_4G, BMA180_BW_10HZ);
/* HMC: */
hmc5883_dev_t hmc;
hmc5883_init(&hmc, &bus);
/* MS: */
ms5611_dev_t ms;
ret = ms5611_init(&ms, &bus, MS5611_OSR4096, MS5611_OSR4096);
if (ret < 0)
{
fatal("could not inizialize MS5611", ret);
return EXIT_FAILURE;
}
pthread_t thread;
pthread_create(&thread, NULL, ms5611_reader, &ms);
/* initialize AHRS filter: */
madgwick_ahrs_t madgwick_ahrs;
madgwick_ahrs_init(&madgwick_ahrs, STANDARD_BETA);
interval_t interval;
interval_init(&interval);
float init = START_BETA;
udp_socket_t *socket = udp_socket_create("10.0.0.100", 5005, 0, 0);
/* kalman filter: */
kalman_t kalman1, kalman2, kalman3;
kalman_init(&kalman1, 1.0e-6, 1.0e-2, 0, 0);
kalman_init(&kalman2, 1.0e-6, 1.0e-2, 0, 0);
kalman_init(&kalman3, 1.0e-6, 1.0e-2, 0, 0);
vec3_t global_acc; /* x = N, y = E, z = D */
int init_done = 0;
int converged = 0;
sliding_avg_t *avg[3];
avg[0] = sliding_avg_create(1000, 0.0);
avg[1] = sliding_avg_create(1000, 0.0);
avg[2] = sliding_avg_create(1000, -9.81);
float alt_rel_last = 0.0;
int udp_cnt = 0;
while (1)
{
int i;
float dt = interval_measure(&interval);
init -= BETA_STEP;
if (init < FINAL_BETA)
{
init = FINAL_BETA;
init_done = 1;
}
madgwick_ahrs.beta = init;
/* sensor data acquisition: */
itg3200_read_gyro(&itg);
bma180_read_acc(&bma);
hmc5883_read(&hmc);
/* state estimates and output: */
euler_t euler;
madgwick_ahrs_update(&madgwick_ahrs, itg.gyro.x, itg.gyro.y, itg.gyro.z, bma.raw.x, bma.raw.y, bma.raw.z, hmc.raw.x, hmc.raw.y, hmc.raw.z, 11.0, dt);
quat_t q_body_to_world;
quat_copy(&q_body_to_world, &madgwick_ahrs.quat);
quat_rot_vec(&global_acc, &bma.raw, &q_body_to_world);
for (i = 0; i < 3; i++)
{
global_acc.vec[i] -= sliding_avg_calc(avg[i], global_acc.vec[i]);
}
if (init_done)
{
kalman_in_t kalman_in;
kalman_in.dt = dt;
kalman_in.pos = 0;
kalman_out_t kalman_out;
kalman_in.acc = global_acc.x;
kalman_run(&kalman_out, &kalman1, &kalman_in);
kalman_in.acc = global_acc.y;
kalman_run(&kalman_out, &kalman2, &kalman_in);
kalman_in.acc = -global_acc.z;
pthread_mutex_lock(&mutex);
kalman_in.pos = alt_rel;
pthread_mutex_unlock(&mutex);
kalman_run(&kalman_out, &kalman3, &kalman_in);
if (!converged)
{
if (fabs(kalman_out.pos - alt_rel) < 0.1)
{
converged = 1;
fprintf(stderr, "init done\n");
}
}
if (converged) // && udp_cnt++ > 10)
{
if (udp_cnt++ == 10)
{
char buffer[1024];
udp_cnt = 0;
int len = sprintf(buffer, "%f %f %f %f %f %f %f", madgwick_ahrs.quat.q0, madgwick_ahrs.quat.q1, madgwick_ahrs.quat.q2, madgwick_ahrs.quat.q3,
global_acc.x, global_acc.y, global_acc.z);
udp_socket_send(socket, buffer, len);
}
printf("%f %f %f\n", -global_acc.z, alt_rel, kalman_out.pos);
fflush(stdout);
}
}
}
return 0;
}
/*
* Application entry point.
*/
int main(void)
{
enum led_status lstat = LST_INIT;
EventListener el0;
alert_status_t proto_st = ALST_INIT;
alert_status_t bmp085_st = ALST_INIT;
alert_status_t mpu6050_st = ALST_INIT;
alert_status_t hmc5883_st = ALST_INIT;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
#ifdef BOARD_IMU_AHRF
/* Clear DRDY pad */
palClearPad(GPIOA, GPIOA_DRDY);
/* Activates serial */
sdStart(&SD1, NULL);
sdStart(&SD2, NULL);
/* Activate pwm */
pwmStart(&PWMD1, &pwm1cfg);
/* Activate i2c */
i2cStart(&I2CD1, &i2c1cfg);
/* Activate exti */
extStart(&EXTD1, &extcfg);
#endif /* BOARD_IMU_AHRF */
#ifdef BOARD_CAPTAIN_PRO2
/* Activates serial */
sdStart(&SD3, NULL);
sdStart(&SD4, NULL);
/* Activate pwm */
pwmStart(&PWMD3, &pwm3cfg);
pwmStart(&PWMD4, &pwm4cfg);
pwmStart(&PWMD5, &pwm5cfg);
/* Activate i2c */
i2cStart(&I2CD1, &i2c1cfg);
/* Activate exti */
extStart(&EXTD1, &extcfg);
/* Activate adc */
adcStart(&ADCD1, NULL);
#endif /* BOARD_CAPTAIN_PRO2 */
/* alert subsys */
chEvtInit(&alert_event_source);
chEvtRegister(&alert_event_source, &el0, 0);
/* init devices */
pt_init();
chThdSleepMilliseconds(10); /* power on delay */
#ifdef HAS_DEV_BMP085
bmp085_init();
chThdSleepMilliseconds(50); /* init delay */
#endif
#ifdef HAS_DEV_MS5611
ms5611_init(&ms5611cfg);
chThdSleepMilliseconds(50); /* init delay */
#endif
#ifdef HAS_DEV_MPU6050
mpu6050_init(&mpu6050cfg);
chThdSleepMilliseconds(250); /* give some time for mpu6050 configuration */
#endif
#ifdef HAS_DEV_HMC5883
hmc5883_init(&hmc5883cfg);
#endif
#ifdef HAS_DEV_SERVOPWM
servopwm_init(&servopwmcfg);
#endif
#ifdef HAS_DEV_NTC10K
ntc10k_init();
#endif
#ifdef HAS_DEV_RPM
rpm_init();
#endif
#ifdef BOARD_IMU_AHRF
/* Set DRDY pad */
palSetPad(GPIOA, GPIOA_DRDY);
#endif
while (TRUE) {
eventmask_t msk = chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(100));
if (msk & EVENT_MASK(0)) {
flagsmask_t fl = chEvtGetAndClearFlags(&el0);
if (fl & ALERT_FLAG_PROTO)
proto_st = pt_get_status();
#ifdef HAS_DEV_MPU6050
if (fl & ALERT_FLAG_MPU6050)
mpu6050_st = mpu6050_get_status();
#endif
#ifdef HAS_DEV_HMC5883
if (fl & ALERT_FLAG_HMC5883)
hmc5883_st = hmc5883_get_status();
#endif
#ifdef HAS_DEV_BMP085
if (fl & ALERT_FLAG_BMP085)
bmp085_st = bmp085_get_status();
#endif
#ifdef HAS_DEV_MS5611
if (fl & ALERT_FLAG_BMP085)
bmp085_st = ms5611_get_status();
#endif
pt_set_sens_state(mpu6050_st, hmc5883_st, bmp085_st);
}
if (proto_st == ALST_FAIL || mpu6050_st == ALST_FAIL || hmc5883_st == ALST_FAIL || bmp085_st == ALST_FAIL)
lstat = LST_FAIL;
else if (proto_st == ALST_INIT || mpu6050_st == ALST_INIT || hmc5883_st == ALST_INIT || bmp085_st == ALST_INIT)
lstat = LST_INIT;
else if (proto_st == ALST_NORMAL && mpu6050_st == ALST_NORMAL && hmc5883_st == ALST_NORMAL && bmp085_st == ALST_NORMAL)
lstat = LST_NORMAL;
led_update(lstat);
}
}
