diff --git a/conf/autopilot/lisa_m_test_progs.makefile b/conf/autopilot/lisa_m_test_progs.makefile index 5461683fa8..59e2c0a6aa 100644 --- a/conf/autopilot/lisa_m_test_progs.makefile +++ b/conf/autopilot/lisa_m_test_progs.makefile @@ -430,7 +430,7 @@ test_imu_aspirin.srcs = $(SRC_AIRBORNE)/mcu.c \ $(SRC_ARCH)/mcu_arch.c \ $(SRC_ARCH)/stm32_exceptions.c \ $(SRC_ARCH)/stm32_vector_table.c\ - test/test_imu.c + test/subsystems/test_imu.c test_imu_aspirin.CFLAGS += -DUSE_LED test_imu_aspirin.srcs += $(SRC_ARCH)/led_hw.c diff --git a/sw/airborne/test/subsystems/test_imu.c b/sw/airborne/test/subsystems/test_imu.c index 97921824d8..5c559e541b 100644 --- a/sw/airborne/test/subsystems/test_imu.c +++ b/sw/airborne/test/subsystems/test_imu.c @@ -1,7 +1,7 @@ /* * $Id$ * - * Copyright (C) 2008-2011 The Paparazzi Team + * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. * @@ -23,6 +23,9 @@ #include +#ifdef BOARD_CONFIG +#include BOARD_CONFIG +#endif #include "std.h" #include "mcu.h" #include "mcu_periph/sys_time.h" @@ -33,7 +36,7 @@ #include "subsystems/imu.h" -#include "my_debug_servo.h" +#include "interrupt_hw.h" static inline void main_init( void ); static inline void main_periodic_task( void ); @@ -46,7 +49,7 @@ static inline void on_mag_event(void); int main( void ) { main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) main_periodic_task(); main_event_task(); } @@ -56,19 +59,24 @@ int main( void ) { static inline void main_init( void ) { mcu_init(); - sys_time_init(); + + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + imu_init(); - // DEBUG_SERVO1_INIT(); - // DEBUG_SERVO2_INIT(); - - mcu_int_enable(); } +static inline void led_toggle ( void ) { + +#ifdef BOARD_LISA_L + LED_TOGGLE(3); +#endif +} + static inline void main_periodic_task( void ) { RunOnceEvery(100, { - // LED_TOGGLE(3); + led_toggle(); DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); }); #ifdef USE_I2C2 @@ -93,6 +101,7 @@ static inline void main_event_task( void ) { ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event); + } static inline void on_accel_event(void) { @@ -118,6 +127,7 @@ static inline void on_accel_event(void) { static inline void on_gyro_accel_event(void) { ImuScaleGyro(imu); + RunOnceEvery(50, LED_TOGGLE(2)); static uint8_t cnt; cnt++; if (cnt > 15) cnt = 0; @@ -156,4 +166,3 @@ static inline void on_mag_event(void) { &imu.mag_unscaled.z); } } - diff --git a/sw/airborne/test/test_imu.c b/sw/airborne/test/test_imu.c deleted file mode 100644 index 5c559e541b..0000000000 --- a/sw/airborne/test/test_imu.c +++ /dev/null @@ -1,168 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 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. - */ - -#include - -#ifdef BOARD_CONFIG -#include BOARD_CONFIG -#endif -#include "std.h" -#include "mcu.h" -#include "mcu_periph/sys_time.h" -#include "led.h" -#include "mcu_periph/uart.h" -#include "messages.h" -#include "subsystems/datalink/downlink.h" - -#include "subsystems/imu.h" - -#include "interrupt_hw.h" - -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); - -static inline void on_gyro_accel_event(void); -static inline void on_accel_event(void); -static inline void on_mag_event(void); - -int main( void ) { - main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) - main_periodic_task(); - main_event_task(); - } - return 0; -} - -static inline void main_init( void ) { - - mcu_init(); - - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); - - imu_init(); - - mcu_int_enable(); -} - -static inline void led_toggle ( void ) { - -#ifdef BOARD_LISA_L - LED_TOGGLE(3); -#endif -} - -static inline void main_periodic_task( void ) { - RunOnceEvery(100, { - led_toggle(); - DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); - }); -#ifdef USE_I2C2 - RunOnceEvery(111, { - DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice, - &i2c2_errors.ack_fail_cnt, - &i2c2_errors.miss_start_stop_cnt, - &i2c2_errors.arb_lost_cnt, - &i2c2_errors.over_under_cnt, - &i2c2_errors.pec_recep_cnt, - &i2c2_errors.timeout_tlow_cnt, - &i2c2_errors.smbus_alert_cnt, - &i2c2_errors.unexpected_event_cnt, - &i2c2_errors.last_unexpected_event); - }); -#endif - if (cpu_time_sec > 1) imu_periodic(); - RunOnceEvery(10, { LED_PERIODIC();}); -} - -static inline void main_event_task( void ) { - - ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event); - - -} - -static inline void on_accel_event(void) { - ImuScaleAccel(imu); - - static uint8_t cnt; - cnt++; - if (cnt > 15) cnt = 0; - if (cnt == 0) { - DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, - &imu.accel_unscaled.x, - &imu.accel_unscaled.y, - &imu.accel_unscaled.z); - } - else if (cnt == 7) { - DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice, - &imu.accel.x, - &imu.accel.y, - &imu.accel.z); - } -} - -static inline void on_gyro_accel_event(void) { - ImuScaleGyro(imu); - - RunOnceEvery(50, LED_TOGGLE(2)); - static uint8_t cnt; - cnt++; - if (cnt > 15) cnt = 0; - - if (cnt == 0) { - DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, - &imu.gyro_unscaled.p, - &imu.gyro_unscaled.q, - &imu.gyro_unscaled.r); - } - else if (cnt == 7) { - DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice, - &imu.gyro.p, - &imu.gyro.q, - &imu.gyro.r); - } -} - - -static inline void on_mag_event(void) { - ImuScaleMag(imu); - static uint8_t cnt; - cnt++; - if (cnt > 10) cnt = 0; - - if (cnt == 0) { - DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice, - &imu.mag.x, - &imu.mag.y, - &imu.mag.z); - } - else if (cnt == 5) { - DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, - &imu.mag_unscaled.x, - &imu.mag_unscaled.y, - &imu.mag_unscaled.z); - } -}