diff --git a/conf/airframes/Poine/beth.xml b/conf/airframes/Poine/beth.xml index 994eea0faf..2d246b6a58 100644 --- a/conf/airframes/Poine/beth.xml +++ b/conf/airframes/Poine/beth.xml @@ -173,6 +173,7 @@ main_stm32.srcs += $(SRC_BETH)/bench_sensors_can.c main_coders.ARCHDIR = stm32 main_coders.LDSCRIPT = $(SRC_ARCH)/stm32f103rb_flash.ld main_coders.OOCD_INTERFACE = olimex-arm-usb-ocd +main_coders.OOCD_BOARD = olimex_stm32_h103 #main_coders.CFLAGS += -DBOARD_CONFIG=\"boards/olimex_stm32-h103.h\" main_coders.CFLAGS += -DBOARD_CONFIG=\"boards/beth.h\" @@ -196,7 +197,13 @@ main_coders.srcs += downlink.c pprz_transport.c #main_coders.CFLAGS += -DUSE_I2C2 #main_coders.srcs += $(SRC_ARCH)/i2c_hw.c -main_coders.CFLAGS += -DUSE_CAN1 -DUSE_USB_LP_CAN1_RX0_IRQ -DBLINKENLIGHTS +main_coders.CFLAGS += -DUSE_CAN1 -DUSE_USB_LP_CAN1_RX0_IRQ -DUSE_CAN_EXT_ID +main_coders.CFLAGS += \ + -DCAN_PRESCALER=11 \ + -DCAN_SJW_TQ=CAN_SJW_1tq \ + -DCAN_BS1_TQ=CAN_BS1_3tq \ + -DCAN_BS2_TQ=CAN_BS2_5tq \ + -DCAN_ERR_RESUME=DISABLE main_coders.srcs += can.c $(SRC_ARCH)/can_hw.c main_coders.srcs += $(SRC_BETH)/bench_sensors_can.c diff --git a/conf/settings/settings_beth.xml b/conf/settings/settings_beth.xml index 5cae3407f3..ded14dbc30 100644 --- a/conf/settings/settings_beth.xml +++ b/conf/settings/settings_beth.xml @@ -9,7 +9,10 @@ - + + + + @@ -18,6 +21,9 @@ + + + diff --git a/sw/airborne/beth/bench_sensors_can.c b/sw/airborne/beth/bench_sensors_can.c index 052570a1c5..a263a14f62 100644 --- a/sw/airborne/beth/bench_sensors_can.c +++ b/sw/airborne/beth/bench_sensors_can.c @@ -45,10 +45,12 @@ static void can_rx_callback(uint32_t id, uint8_t *buf, int len) { tempangle = (buf[3]<<8) | buf[2]; if ((tempangle == 0 )||(tempangle > 6000)) {can_err_flags = 0x20;} else {bench_sensors.angle_3 = tempangle;} rx_bd2 = 0; + LED_TOGGLE(4); } else { tempangle = (buf[3]<<8) | buf[2]; if ((tempangle == 0 )||(tempangle > 6000)) {can_err_flags = 0x10;} else {bench_sensors.angle_1 = tempangle;} rx_bd1 = 0; + //LED_TOGGLE(5); } } diff --git a/sw/airborne/beth/main_coders.c b/sw/airborne/beth/main_coders.c index 2d9e7b71a8..078da21ad6 100644 --- a/sw/airborne/beth/main_coders.c +++ b/sw/airborne/beth/main_coders.c @@ -34,7 +34,7 @@ static inline void main_periodic( void ); static inline void main_event( void ); static inline void main_init_adc(void); -static inline void main_on_bench_sensors( void ); +//static inline void main_on_bench_sensors( void ); //static inline void main_init_i2c2(void); //void i2c2_ev_irq_handler(void); @@ -76,39 +76,37 @@ static inline void main_init( void ) { hw_init(); sys_time_init(); main_init_adc(); - //main_init_i2c2(); - //i2c2_init(); - can_init(); + bench_sensors_init(); int_enable(); - } static inline void main_periodic( void ) { - RunOnceEvery(10, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);}); + /*RunOnceEvery(10, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});*/ //RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &coder_values[0], &coder_values[1]);}); //RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &can1_status, &can1_pending);}); - RunOnceEvery(5, {DOWNLINK_SEND_BETH(DefaultChannel, &bench_sensors.angle_1, - &bench_sensors.angle_2,&bench_sensors.angle_3, &bench_sensors.current);}); + /*RunOnceEvery(5, {DOWNLINK_SEND_BETH(DefaultChannel, &bench_sensors.angle_1, + &bench_sensors.angle_2,&bench_sensors.angle_3, &bench_sensors.current);});*/ servos[0]=coder_values[0]; servos[1]=coder_values[1]; //use id=1 for azimuth board - can_transmit(1, 0, (uint8_t *)servos, 8); - + can_transmit(1, (uint8_t *)servos, 8); + LED_TOGGLE(5); } static inline void main_event( void ) { - BenchSensorsEvent(main_on_bench_sensors); + //BenchSensorsEvent(main_on_bench_sensors); } -static inline void main_on_bench_sensors( void ) { +/*static inline void main_on_bench_sensors( void ) { -} +}*/ + #if 0 /* diff --git a/sw/airborne/beth/main_stm32.c b/sw/airborne/beth/main_stm32.c index 6dc4654a99..768cf648cd 100644 --- a/sw/airborne/beth/main_stm32.c +++ b/sw/airborne/beth/main_stm32.c @@ -98,9 +98,6 @@ static inline void main_periodic( void ) { overo_link.up.msg.thrust_out = thrust_out; overo_link.up.msg.pitch_out = pitch_out; - booz2_commands[COMMAND_PITCH] = pitch_out; - booz2_commands[COMMAND_THRUST] = thrust_out; - //stop the motors if we've lost SPI or CAN link //If SPI has had CRC error we don't stop motors if ((spi_msg_cnt == 0) || (can_err_flags != 0)) { @@ -110,6 +107,8 @@ static inline void main_periodic( void ) { overo_link.up.msg.can_errs = can_err_flags; overo_link.up.msg.pitch_out = PITCH_MAGIC_NUMBER; } else { + booz2_commands[COMMAND_PITCH] = pitch_out; + booz2_commands[COMMAND_THRUST] = thrust_out; actuators_set(TRUE); } } diff --git a/sw/airborne/beth/overo_estimator.c b/sw/airborne/beth/overo_estimator.c index ab87e7a902..daac9d1937 100644 --- a/sw/airborne/beth/overo_estimator.c +++ b/sw/airborne/beth/overo_estimator.c @@ -6,8 +6,9 @@ struct OveroEstimator estimator; void estimator_init(void) { - estimator.tilt_lp_coeff = 0.9; - estimator.elevation_lp_coeff = 0.9; + estimator.tilt_lp_coeff = 0.5; + estimator.elevation_lp_coeff = 0.5; + estimator.azimuth_lp_coeff = 0.5; } //bench sensors z,y,x values passed in @@ -23,10 +24,10 @@ void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t a estimator.tilt = -(tilt_neutral - (int32_t)tilt_measure ) * tilt_scale; Bound(estimator.tilt,-89,89); - //low pass filter gyro values + //low pass filter tilt gyro estimator.tilt_dot = estimator.tilt_dot + estimator.tilt_lp_coeff * (RATE_FLOAT_OF_BFP(booz_imu.gyro.q) - estimator.tilt_dot); - /* Second order filter to be tested + /* Second order filter yet to be tested estimator.tilt_dot = estimator.tilt_dot * (2 - estimator.tilt_lp_coeff1 - estimator.tilt_lp_coeff2) + estimator.tilt_lp_coeff1 * estimator.tilt_lp_coeff2 * RATE_FLOAT_OF_BFP(booz_imu.gyro.q) - estimator.tilt_dot_old * (1 - estimator.tilt_lp_coeff1 - estimator.tilt_lp_coeff2 + @@ -44,7 +45,11 @@ void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t a estimator.elevation_lp_coeff * (rotated_elev_dot - estimator.elevation_dot); estimator.azimuth = (azimuth_neutral - (int32_t)azimuth_measure ) * azimuth_scale; - estimator.azimuth_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.r); + + //low pass filter azimuth gyro + //TODO: compensate rotation and increase order + estimator.azimuth_dot = estimator.azimuth_dot + + estimator.azimuth_lp_coeff * (RATE_FLOAT_OF_BFP(booz_imu.gyro.r) - estimator.azimuth_dot); } diff --git a/sw/airborne/beth/overo_estimator.h b/sw/airborne/beth/overo_estimator.h index 94056c048e..a5214ee0c9 100644 --- a/sw/airborne/beth/overo_estimator.h +++ b/sw/airborne/beth/overo_estimator.h @@ -15,6 +15,7 @@ struct OveroEstimator { float tilt_lp_coeff; float elevation_lp_coeff; + float azimuth_lp_coeff; };