diff --git a/conf/autopilot/conf_booz.makefile b/conf/autopilot/conf_booz.makefile index c37404c2e7..683dcdd859 100644 --- a/conf/autopilot/conf_booz.makefile +++ b/conf/autopilot/conf_booz.makefile @@ -84,6 +84,7 @@ flt.CFLAGS += -DDISABLE_MAGNETOMETER flt.srcs += micromag.c $(SRC_ARCH)/micromag_hw.c flt.srcs += imu_v3.c $(SRC_ARCH)/imu_v3_hw.c +flt.CFLAGS += -DBOOZ_AHRS_TYPE=BOOZ_AHRS_MULTITILT flt.srcs += multitilt.c flt.srcs += booz_inter_mcu.c diff --git a/conf/telemetry/booz.xml b/conf/telemetry/booz.xml index b4916065cb..5269b0d9a1 100644 --- a/conf/telemetry/booz.xml +++ b/conf/telemetry/booz.xml @@ -6,6 +6,7 @@ + @@ -26,6 +27,17 @@ + + + + + + + + + + + @@ -37,12 +49,7 @@ - - - - - - + diff --git a/sw/airborne/arm7/max1167_hw.c b/sw/airborne/arm7/max1167_hw.c index 75030c1ce8..bbb0a66874 100644 --- a/sw/airborne/arm7/max1167_hw.c +++ b/sw/airborne/arm7/max1167_hw.c @@ -1,10 +1,16 @@ #include "max1167.h" +#include "led.h" + +uint8_t max1167_error; static void EXTINT0_ISR(void) __attribute__((naked)); extern void max1167_hw_init( void ) { - /* CS pin is output */ + + max1167_error = MAX1167_ERR_NONE; + + /* SS pin is output */ SetBit(MAX1167_SS_IODIR, MAX1167_SS_PIN); /* unselected max1167 */ Max1167Unselect(); @@ -23,11 +29,12 @@ extern void max1167_hw_init( void ) { VICIntEnable = VIC_BIT( VIC_EINT0 ); // EXTINT0 interrupt enabled VICVectCntl8 = VIC_ENABLE | VIC_EINT0; VICVectAddr8 = (uint32_t)EXTINT0_ISR; // address of the ISR - - - } +#include "uart.h" +#include "messages.h" +#include "downlink.h" + void max1167_read( void ) { if (max1167_status == STA_MAX1167_IDLE) { /* select max1167 */ @@ -38,19 +45,22 @@ void max1167_read( void ) { const uint8_t control_byte = 1 << 0 | 1 << 3 | 2 << 5; SpiSend(control_byte); /* enable timeout interrupt */ - SpiEnableRti(); + // SpiClearRti(); + // SpiEnableRti(); max1167_status = STA_MAX1167_SENDING_REQ; } else { /* report overrun error */ + max1167_error = MAX1167_ERR_READ_OVERUN; + DOWNLINK_SEND_DEBUG_MCU_LINK(&max1167_status,&max1167_error, &max1167_status); } } void EXTINT0_ISR(void) { ISR_ENTRY(); - - if (max1167_status == STA_MAX1167_WAIT_EOC) { - /* Max1167Select(); maybe... */ + + // if (max1167_status == STA_MAX1167_WAIT_EOC) { + if (max1167_status == STA_MAX1167_SENDING_REQ) { SpiEnable(); /* trigger 6 bytes read */ SpiSend(0); @@ -67,6 +77,12 @@ void EXTINT0_ISR(void) { } else { /* report error */ + max1167_error = MAX1167_ERR_SPURIOUS_EOC; + DOWNLINK_SEND_DEBUG_MCU_LINK(&max1167_status,&max1167_error, &max1167_status); + /* reset */ + max1167_status = STA_MAX1167_IDLE; + SpiDisable(); + Max1167Unselect(); } SetBit(EXTINT, MAX1167_EOC_EINT); /* clear extint0 */ diff --git a/sw/airborne/arm7/max1167_hw.h b/sw/airborne/arm7/max1167_hw.h index b3c9604ce7..b2a7ebf433 100644 --- a/sw/airborne/arm7/max1167_hw.h +++ b/sw/airborne/arm7/max1167_hw.h @@ -12,6 +12,10 @@ #include "interrupt_hw.h" #include "spi_hw.h" +#define MAX1167_ERR_NONE 0 +#define MAX1167_ERR_READ_OVERUN 1 +#define MAX1167_ERR_SPURIOUS_EOC 2 +extern uint8_t max1167_error; #define MAX1167_SS_PIN 29 #define MAX1167_SS_IODIR IO1DIR @@ -42,7 +46,6 @@ SpiClearRti(); \ SpiDisableRti(); \ SpiDisable(); \ - /* Max1167Unselect(); maybe... */ \ /* here we might enable extint0 */ \ max1167_status = STA_MAX1167_WAIT_EOC; \ } \ @@ -52,25 +55,29 @@ /* should not happen */ \ break; \ \ - case STA_MAX1167_READING_RES: \ - /* store convertion result */ \ - max1167_values[0] = SSPDR << 8; \ - max1167_values[0] += SSPDR; \ - max1167_values[1] = SSPDR << 8; \ - max1167_values[1] += SSPDR; \ - max1167_values[2] = SSPDR << 8; \ - max1167_values[2] += SSPDR; \ - SpiClearRti(); \ - SpiDisableRti(); \ - SpiDisable(); \ - Max1167Unselect(); \ - max1167_status = STA_MAX1167_DATA_AVAILABLE; \ - break; \ - case STA_MAX1167_DATA_AVAILABLE : \ - /* should not happen */ \ - break; \ - } \ - } \ -} + case STA_MAX1167_READING_RES: { \ + /* read dummy control byte reply */ \ + uint8_t foo __attribute__ ((unused)); \ + foo = SSPDR; \ + /* store convertion result */ \ + max1167_values[0] = SSPDR << 8; \ + max1167_values[0] += SSPDR; \ + max1167_values[1] = SSPDR << 8; \ + max1167_values[1] += SSPDR; \ + max1167_values[2] = SSPDR << 8; \ + max1167_values[2] += SSPDR; \ + SpiClearRti(); \ + SpiDisableRti(); \ + SpiDisable(); \ + Max1167Unselect(); \ + max1167_status = STA_MAX1167_DATA_AVAILABLE; \ + } \ + break; \ + case STA_MAX1167_DATA_AVAILABLE : \ + /* should not happen */ \ + break; \ + } \ + } \ + } #endif /* MAX1167_WH */ diff --git a/sw/airborne/booz_ahrs.h b/sw/airborne/booz_ahrs.h new file mode 100644 index 0000000000..32053ae990 --- /dev/null +++ b/sw/airborne/booz_ahrs.h @@ -0,0 +1,40 @@ +#ifndef BOOZ_AHRS_H +#define BOOZ_AHRS_H + +#define BOOZ_AHRS_STATUS_UNINIT 0 +#define BOOZ_AHRS_STATUS_RUNNING 1 +#define BOOZ_AHRS_STATUS_CRASHED 2 + +extern uint8_t booz_ahrs_status; + +extern float booz_ahrs_phi; +extern float booz_ahrs_theta; +extern float booz_ahrs_psi; + +extern float booz_ahrs_p; +extern float booz_ahrs_q; +extern float booz_ahrs_r; + +extern float booz_ahrs_bp; +extern float booz_ahrs_bq; +extern float booz_ahrs_br; + +extern void booz_ahrs_init(void); +extern void booz_ahrs_start( const float* accel, const float* gyro, const int16_t* mag); +extern void booz_ahrs_predict( const float* gyro ); +extern void booz_ahrs_update( const float* accel, const int16_t* mag ); + +//#define __AHRS_IMPL_HEADER(_typ, _ext) _t##_ext +//#define _AHRS_IMPL_HEADER(_typ, _ext) __AHRS_IMPL_HEADER(_typ, _ext) +//#define AHRS_IMPL_HEADER(_typ) _AHRS_IMPL_HEADER(_typ, ".h") +//#error BOOZ_AHRS_TYPE +//#include \"AHRS_IMPL_HEADER(BOOZ_AHRS_TYPE)\" + +#define BOOZ_AHRS_MULTITILT 0 +#define BOOZ_AHRS_EULER 1 + +#if defined BOOZ_AHRS_TYPE && BOOZ_AHRS_TYPE == BOOZ_AHRS_MULTITILT +#include "multitilt.h" +#endif + +#endif /* BOOZ_AHRS_H */ diff --git a/sw/airborne/booz_autopilot.c b/sw/airborne/booz_autopilot.c index 8398e36cf6..7b6168b63a 100644 --- a/sw/airborne/booz_autopilot.c +++ b/sw/airborne/booz_autopilot.c @@ -40,6 +40,7 @@ void booz_autopilot_on_rc_event(void) { /* the ap gets a mode everytime - the rc filters it */ if (rc_values_contains_avg_channels) { booz_autopilot_mode = BOOZ_AP_MODE_OF_PPRZ(rc_values[RADIO_MODE]); + rc_values_contains_avg_channels = FALSE; } switch (booz_autopilot_mode) { case BOOZ_AP_MODE_RATE: diff --git a/sw/airborne/booz_filter_main.c b/sw/airborne/booz_filter_main.c index 57d69a93be..c66217d30b 100644 --- a/sw/airborne/booz_filter_main.c +++ b/sw/airborne/booz_filter_main.c @@ -7,7 +7,9 @@ #include "adc.h" #include "imu_v3.h" -#include "multitilt.h" + +//#include "multitilt.h" +#include "booz_ahrs.h" #include "uart.h" #include "messages.h" diff --git a/sw/airborne/booz_inter_mcu.c b/sw/airborne/booz_inter_mcu.c index f235f30187..b45c3888db 100644 --- a/sw/airborne/booz_inter_mcu.c +++ b/sw/airborne/booz_inter_mcu.c @@ -7,10 +7,19 @@ struct booz_inter_mcu_state inter_mcu_state; #ifdef BOOZ_FILTER_MCU + +#define LP_GYROS + void inter_mcu_fill_state() { +#ifndef LP_GYROS inter_mcu_state.r_rates[AXIS_P] = imu_vs_gyro_unbiased[AXIS_P] * RATE_PI_S/M_PI; inter_mcu_state.r_rates[AXIS_Q] = imu_vs_gyro_unbiased[AXIS_Q] * RATE_PI_S/M_PI; inter_mcu_state.r_rates[AXIS_R] = imu_vs_gyro_unbiased[AXIS_R] * RATE_PI_S/M_PI; +#else + inter_mcu_state.r_rates[AXIS_P] = imu_gyro_lp[AXIS_P] * RATE_PI_S/M_PI; + inter_mcu_state.r_rates[AXIS_Q] = imu_gyro_lp[AXIS_Q] * RATE_PI_S/M_PI; + inter_mcu_state.r_rates[AXIS_R] = imu_gyro_lp[AXIS_R] * RATE_PI_S/M_PI; +#endif inter_mcu_state.f_rates[AXIS_P] = mtt_p * RATE_PI_S/M_PI; inter_mcu_state.f_rates[AXIS_Q] = mtt_q * RATE_PI_S/M_PI; inter_mcu_state.f_rates[AXIS_R] = mtt_r * RATE_PI_S/M_PI; diff --git a/sw/airborne/booz_telemetry.h b/sw/airborne/booz_telemetry.h index d04ae73727..8b8e6c63de 100644 --- a/sw/airborne/booz_telemetry.h +++ b/sw/airborne/booz_telemetry.h @@ -29,6 +29,9 @@ &cpu_time_sec, \ &buss_twi_blmc_nb_err) + +#define PERIODIC_SEND_RC() DOWNLINK_SEND_RC(PPM_NB_PULSES, rc_values) + #define PERIODIC_SEND_BOOZ_FD() \ DOWNLINK_SEND_BOOZ_FD(&booz_estimator_p, \ &booz_estimator_q, \ diff --git a/sw/airborne/control_grz.h b/sw/airborne/control_grz.h deleted file mode 100644 index ee8fa17a92..0000000000 --- a/sw/airborne/control_grz.h +++ /dev/null @@ -1,93 +0,0 @@ -/* - * Paparazzi $Id$ - * - * Copyright (C) 2006 Pascal Brisset, Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#ifndef CONTROL_GRZ_H -#define CONTROL_GRZ_H - -#include - -struct pid { - float measure; - float set_point; - float p_gain; - float i_gain; - float d_gain; - float last_err; - float sum_err; - float saturation; -}; - -extern float max_roll_dot_sp; -extern float max_pitch_dot_sp; -extern float max_yaw_dot_sp; - -extern float ctl_grz_throttle_level; -extern float ctl_grz_z_dot; -extern float ctl_grz_z; -extern float ctl_grz_z_dot_sum_err; -extern float ctl_grz_z_sum_err; - -extern float ctl_grz_z_dot_pgain; -extern float ctl_grz_z_dot_igain; -extern float ctl_grz_z_dot_dgain; -extern float ctl_grz_z_dot_setpoint; - -extern float ctl_grz_z_pgain; -extern float ctl_grz_z_igain; -extern float ctl_grz_z_dgain; -extern float ctl_grz_z_setpoint; - -extern struct pid roll_pid; -extern struct pid pitch_pid; -extern struct pid yaw_pid; - -extern struct pid roll_dot_pid; -extern struct pid pitch_dot_pid; -extern struct pid yaw_dot_pid; - -extern void ctl_grz_init( void ); -extern void ctl_grz_set_setpoints_rate( void ); -extern void ctl_grz_set_setpoints_auto1( void ); -extern void ctl_grz_set_setpoints_auto2( void ); -extern void ctl_grz_set_measures( void ); -extern void ctl_grz_rate_run ( void ); -extern void ctl_grz_attitude_run ( void ); - -extern void ctl_grz_z_dot_run ( void ); -extern void ctl_grz_alt_run ( void ); -extern void ctl_grz_horiz_speed_run ( void ); - -extern float north_angle_set_point; -extern float east_angle_set_point; -extern struct pid vn_pid; -extern struct pid ve_pid; - -extern bool_t flying; - -void ctl_grz_reset( void ); - -extern float trim_roll, trim_yaw, trim_pitch; - - -#endif // CONTROL_GRZ_H diff --git a/sw/airborne/imu_v3.h b/sw/airborne/imu_v3.h index a673bc535d..f020f2b355 100644 --- a/sw/airborne/imu_v3.h +++ b/sw/airborne/imu_v3.h @@ -15,7 +15,7 @@ extern int16_t imu_mag[AXIS_NB]; /* magnetometer in arbitrary unit */ extern float imu_bat; /* battery in volts */ extern float imu_gyro_prev[AXIS_NB]; /* previous gyros in rad/s */ -#define IMU_GYRO_LP_ALPHA 0.6 +#define IMU_GYRO_LP_ALPHA 0.1 extern float imu_gyro_lp[AXIS_NB]; /* low passed calibrated gyros in rad/s */ /* raw sensors readings */ @@ -145,13 +145,13 @@ extern struct adc_buf buf_bat; } #define ImuPeriodic() { \ - if (max1167_status != STA_MAX1167_IDLE) { \ + /* if (max1167_status != STA_MAX1167_IDLE) { \ DOWNLINK_SEND_AHRS_OVERRUN(); \ } \ - else { \ + else { */ \ max1167_read(); \ micromag_read(); \ - } \ + /* } */ \ } diff --git a/sw/airborne/max1167.c b/sw/airborne/max1167.c index 77b14689c0..e8b4123212 100644 --- a/sw/airborne/max1167.c +++ b/sw/airborne/max1167.c @@ -1,15 +1,15 @@ #include "max1167.h" -//volatile uint8_t max1167_data_available; volatile uint8_t max1167_status; uint16_t max1167_values[MAX1167_NB_CHAN]; extern void max1167_init( void ) { + max1167_hw_init(); + uint8_t i; for (i=0; ive[AXIS_R] = (double)bsm.gyro_resolution * 0.6035156; /* ratio of full scale - nominal 0.5 */ bsm.gyro_noise_std_dev = v_get(AXIS_NB); - bsm.gyro_noise_std_dev->ve[AXIS_P] = RadOfDeg(1.); - bsm.gyro_noise_std_dev->ve[AXIS_Q] = RadOfDeg(1.); - bsm.gyro_noise_std_dev->ve[AXIS_R] = RadOfDeg(1.); + bsm.gyro_noise_std_dev->ve[AXIS_P] = RadOfDeg(.5); + bsm.gyro_noise_std_dev->ve[AXIS_Q] = RadOfDeg(.5); + bsm.gyro_noise_std_dev->ve[AXIS_R] = RadOfDeg(.5); bsm.gyro_bias_initial = v_get(AXIS_NB); - bsm.gyro_bias_initial->ve[AXIS_P] = RadOfDeg( 1.0); - bsm.gyro_bias_initial->ve[AXIS_Q] = RadOfDeg(-0.25); - bsm.gyro_bias_initial->ve[AXIS_R] = RadOfDeg( 0.5); + bsm.gyro_bias_initial->ve[AXIS_P] = RadOfDeg( -0.2); + bsm.gyro_bias_initial->ve[AXIS_Q] = RadOfDeg( -0.5); + bsm.gyro_bias_initial->ve[AXIS_R] = RadOfDeg( 0.5); bsm.gyro_bias_random_walk_std_dev = v_get(AXIS_NB); bsm.gyro_bias_random_walk_std_dev->ve[AXIS_P] = RadOfDeg(5.e-1); diff --git a/sw/simulator/main_booz_sim.c b/sw/simulator/main_booz_sim.c index 49c592d3cb..639e6a8d49 100644 --- a/sw/simulator/main_booz_sim.c +++ b/sw/simulator/main_booz_sim.c @@ -272,10 +272,10 @@ static void booz_sim_set_ppm_from_joystick( void ) { // printf("joystick roll %f %d\n", booz_joystick_value[JS_ROLL], ppm_pulses[RADIO_ROLL]); #else ppm_pulses[RADIO_THROTTLE] = 1223 + 0.6 * (2050-1223); - BREAK_MTT(); - WALK_OVAL(); + //BREAK_MTT(); + //WALK_OVAL(); // CIRCLE(); - // STATIONARY(); + STATIONARY(); // TOUPIE(); #endif }