diff --git a/conf/airframes/example_heli_lisam.xml b/conf/airframes/example_heli_lisam.xml new file mode 100644 index 0000000000..616a32ed0a --- /dev/null +++ b/conf/airframes/example_heli_lisam.xml @@ -0,0 +1,259 @@ + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + +
+ + +
+ + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + +
+ +
+ + + + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index 78fd75ff8a..7ebebd2c69 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -23,11 +23,8 @@ - + - diff --git a/conf/autopilot/subsystems/rotorcraft/stabilization_int_quat.makefile b/conf/autopilot/subsystems/rotorcraft/stabilization_int_quat.makefile new file mode 100644 index 0000000000..8c00152cab --- /dev/null +++ b/conf/autopilot/subsystems/rotorcraft/stabilization_int_quat.makefile @@ -0,0 +1,5 @@ +ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT +ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\" +ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_quat_int.h\" +ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c +ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c diff --git a/conf/autopilot/subsystems/rotorcraft/stabilization_int_quat_transition.makefile b/conf/autopilot/subsystems/rotorcraft/stabilization_int_quat_transition.makefile new file mode 100644 index 0000000000..2403f25272 --- /dev/null +++ b/conf/autopilot/subsystems/rotorcraft/stabilization_int_quat_transition.makefile @@ -0,0 +1,7 @@ +ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT +ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\" +ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_quat_int.h\" +ap.CFLAGS += -DUSE_SETPOINTS_WITH_TRANSITIONS +ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c +ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c +ap.srcs += $(SRC_FIRMWARE)/stabilization/quat_setpoint_int.c diff --git a/conf/autopilot/subsystems/rotorcraft/stabilization_quaternion.makefile b/conf/autopilot/subsystems/rotorcraft/stabilization_quaternion.makefile index e9d4cc7edd..344044bbe6 100644 --- a/conf/autopilot/subsystems/rotorcraft/stabilization_quaternion.makefile +++ b/conf/autopilot/subsystems/rotorcraft/stabilization_quaternion.makefile @@ -1,6 +1 @@ -ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT -ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\" -ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_quat_int.h\" -ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c -ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c -ap.srcs += $(SRC_FIRMWARE)/stabilization/quat_setpoint.c +$(error The stabilization quaternion subsystem has been changed, for normal rotorcrafts please use , for transitioning rotorcrafts (quadshot) use ) diff --git a/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h b/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h index 4550633116..d0cfee8013 100644 --- a/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h +++ b/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h @@ -55,9 +55,10 @@ extern void sys_tick_irq_handler(void); /** Busy wait, in microseconds */ -/* for now empty shell */ +// FIXME: directly use the SysTick->VAL here static inline void sys_time_usleep(uint32_t us) { - + uint32_t end = GET_CUR_TIME_USEC() + us; + while ((uint32_t)GET_CUR_TIME_USEC() < end); } #endif /* SYS_TIME_ARCH_H */ diff --git a/sw/airborne/boards/lisa_l/baro_board.c b/sw/airborne/boards/lisa_l/baro_board.c index 0333505ba8..43c1414907 100644 --- a/sw/airborne/boards/lisa_l/baro_board.c +++ b/sw/airborne/boards/lisa_l/baro_board.c @@ -66,7 +66,14 @@ void baro_periodic(void) { void baro_board_send_config_abs(void) { +#ifndef BARO_LOW_GAIN +#pragma message "Using High LisaL Baro Gain: Do not use below 1000hPa" baro_board_write_to_register(BARO_ABS_ADDR, 0x01, 0x86, 0x83); +#else +#pragma message "Using Low LisaL Baro Gain, capable of measuring below 1000hPa or more" + //config register should be 0x84 in low countries, or 0x86 in normal countries + baro_board_write_to_register(BARO_ABS_ADDR, 0x01, 0x84, 0x83); +#endif } void baro_board_send_config_diff(void) { diff --git a/sw/airborne/csc/arm7/props_csc.h b/sw/airborne/csc/arm7/props_csc.h deleted file mode 100644 index 3dce09844f..0000000000 --- a/sw/airborne/csc/arm7/props_csc.h +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef PROPS_CSC_H -#define PROPS_CSC_H - -#include "generated/airframe.h" -#include "firmwares/rotorcraft/actuators.h" -#include "mcu_periph/sys_time.h" -#include "csc_ap_link.h" -#include "csc_msg_def.h" - -extern uint8_t csc_prop_speeds[PROPS_NB]; - -void props_set(uint8_t i, uint8_t speed); -void props_commit(); - -#endif /* PROPS_CSC_H */ diff --git a/sw/airborne/csc/csc_adc.c b/sw/airborne/csc/csc_adc.c deleted file mode 100644 index ca4d2db9b4..0000000000 --- a/sw/airborne/csc/csc_adc.c +++ /dev/null @@ -1,51 +0,0 @@ -#include "csc_adc.h" -#include "csc_ap_link.h" -#include -#include "mcu_periph/uart.h" -#include "print.h" - -#include "LPC21xx.h" -#include "led.h" -#include "mcu_periph/adc.h" -#include ACTUATORS -#include "csc_servos.h" -#include "mcu_periph/sys_time.h" - -#define ADC_NB_CSC 4 - -struct adc_buf adc_bufs[ADC_NB_CSC]; - -uint8_t vsupply; -uint16_t adc_slider; -uint16_t adc_values[ADC_NB_CSC]; - -#define ADC_VDIV 5.7 -#define ADC_VOLT 3.28 -#define ADC_FACTOR 1024.0 * ADC_VOLT * ADC_VDIV - -#ifndef ADC_AV_NB -#define ADC_AV_NB 8 -#endif - -#define ADC_VSUPPLY 0 -#define ADC_SLIDER 3 - - - -void csc_adc_init(void) -{ - adc_init(); - for (int i = 0; i < ADC_NB_CSC; i++) { - adc_buf_channel(i, &adc_bufs[i], ADC_AV_NB); - } -} - -void csc_adc_periodic(void) -{ - vsupply = adc_bufs[ADC_VSUPPLY].sum / adc_bufs[ADC_VSUPPLY].av_nb_sample / ADC_FACTOR * 10; - adc_slider = adc_bufs[ADC_SLIDER].sum / adc_bufs[ADC_SLIDER].av_nb_sample; - for (int i = 0; i < ADC_NB_CSC; i++) { - adc_values[i] = adc_bufs[i].sum / adc_bufs[i].av_nb_sample; - } -} - diff --git a/sw/airborne/csc/csc_ap_main.c b/sw/airborne/csc/csc_ap_main.c deleted file mode 100644 index 757dcbd75c..0000000000 --- a/sw/airborne/csc/csc_ap_main.c +++ /dev/null @@ -1,221 +0,0 @@ -/* - * $Id: booz2_main.c 3049 2009-02-24 16:51:25Z poine $ - * - * Copyright (C) 2008 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 - -#include "csc_main.h" - -#include "std.h" - -#include "mcu.h" -#include "mcu_periph/sys_time.h" -#include "led.h" -#include "interrupt_hw.h" -#include "mcu_periph/uart.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic.h" -#include "generated/airframe.h" -#include "commands.h" -#include "subsystems/radio_control.h" -#include "booz/booz2_gps.h" - -//#include "ap_subsystems/datalink/downlink.h" - -#include "csc_servos.h" -#include "csc_telemetry.h" -#include "csc_adc.h" -#include "csc_xsens.h" -#include "csc_autopilot.h" -#include "csc_can.h" -#include "pwm_input.h" -#include "csc_ap_link.h" -#include "led.h" - -#include "subsystems/datalink/pprz_transport.h" - -#define CSC_STATUS_TIMEOUT (CPU_TICKS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD) - -#define PPRZ_MODE_MANUAL 0 -#define PPRZ_MODE_AUTO1 1 - -#define TRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2) -#define PPRZ_MODE_OF_RC(mode) ((mode) < TRESHOLD_MANUAL_PPRZ ? PPRZ_MODE_MANUAL : PPRZ_MODE_AUTO1) - -extern uint8_t vsupply; - -uint8_t pprz_mode = PPRZ_MODE_AUTO1; -static uint16_t cpu_time = 0; -uint8_t csc_trims_set = 0; - -struct Booz_gps_state booz_gps_state; -struct NedCoor_i booz_ins_gps_pos_cm_ned; -struct NedCoor_i booz_ins_gps_speed_cm_s_ned; - -static void nop(struct CscCanMsg *msg) -{ - -} - -#define RADIO_SCALE 20 -#define ROLL_OFFSET 1544 -#define PITCH_OFFSET 2551 -#define YAW_OFFSET 3585 -#define MODE_OFFSET 5632 - -static void on_rc_cmd(struct CscRCMsg *msg) -{ - int aux2_flag; - static int last_aux2_flag = -1; - - rc_values[RADIO_ROLL] = CSC_RC_SCALE*(msg->right_stick_horizontal - CSC_RC_OFFSET); - rc_values[RADIO_PITCH] = -CSC_RC_SCALE*(msg->right_stick_vertical - CSC_RC_OFFSET); - rc_values[RADIO_YAW] = CSC_RC_SCALE*((msg->left_stick_horizontal_and_aux2 & ~(3 << 13)) - CSC_RC_OFFSET); - uint8_t mode = (msg->left_stick_vertical_and_flap_mix & (3 << 13)) >> 13; - rc_values[RADIO_MODE] = mode ? -7000 : ( (mode == 1) ? 0 : 7000); - aux2_flag = (msg->left_stick_horizontal_and_aux2 >> 13) & 0x1; - rc_values[RADIO_MODE2] = (aux2_flag == 0) ? -7000 : ( (aux2_flag == 1) ? 0 : 7000); - rc_values[RADIO_THROTTLE] = -CSC_RC_SCALE*((msg->left_stick_vertical_and_flap_mix & ~(3 << 13)) - CSC_RC_OFFSET); - - time_since_last_ppm = 0; - rc_status = RC_OK; - pprz_mode = PPRZ_MODE_OF_RC(rc_values[RADIO_MODE]); - if (pprz_mode == PPRZ_MODE_MANUAL) { - csc_ap_clear_ierrors(); - SetCommandsFromRC(commands, rc_values); - } -} - -static void on_gpsacc_cmd( struct CscGPSAccMsg *msg ) -{ - booz_gps_state.pacc = msg->pacc; - booz_gps_state.sacc = msg->sacc; -} - -static void on_gpsfix_cmd( struct CscGPSFixMsg *msg ) -{ - booz_gps_state.fix = msg->gps_fix; - booz_gps_state.num_sv = msg->num_sv; - booz_ins_gps_speed_cm_s_ned.x = msg->vx; - booz_ins_gps_speed_cm_s_ned.y = msg->vy; - booz_ins_gps_speed_cm_s_ned.z = msg->vz; -} - -static void on_gpspos_cmd( struct CscGPSPosMsg *msg ) -{ - switch (msg->axis) { - case CSC_GPS_AXIS_IDX_X: - booz_ins_gps_pos_cm_ned.x = msg->val; - break; - case CSC_GPS_AXIS_IDX_Y: - booz_ins_gps_pos_cm_ned.y = msg->val; - break; - case CSC_GPS_AXIS_IDX_Z: - booz_ins_gps_pos_cm_ned.z = msg->val; - break; - default: - // Invalid msg - break; - } -} - -static void csc_main_init( void ) { - - mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); - led_init(); - - Uart0Init(); - Uart1Init(); - - xsens_init(); - - csc_adc_init(); - ppm_init(); - -#ifdef USE_PWM_INPUT - pwm_input_init(); -#endif - - csc_ap_link_init(); - csc_ap_link_set_servo_cmd_cb(&nop); - csc_ap_link_set_motor_cmd_cb(&nop); - csc_ap_link_set_rc_cmd_cb(on_rc_cmd); - csc_ap_link_set_gpsfix_cb(on_gpsfix_cmd); - csc_ap_link_set_gpspos_cb(on_gpspos_cmd); - csc_ap_link_set_gpsacc_cb(on_gpsacc_cmd); - actuators_init(); - - csc_ap_init(); - mcu_int_enable(); - -} - - -static void csc_main_periodic( void ) -{ - static uint32_t csc_loops = 0; - - PeriodicSendAp(DefaultChannel); - radio_control_periodic_task(); - - if (rc_status == RC_REALLY_LOST) { - pprz_mode = PPRZ_MODE_AUTO1; - } - cpu_time++; - - if ((++csc_loops % CSC_STATUS_TIMEOUT) == 0) { - csc_adc_periodic(); - } - xsens_periodic_task(); - if (pprz_mode == PPRZ_MODE_AUTO1) - csc_ap_periodic(cpu_time, &booz_ins_gps_pos_cm_ned, &booz_ins_gps_speed_cm_s_ned); - - if (csc_trims_set) { - csc_trims_set = 0; - csc_ap_set_trims(); - } - -#ifdef ACTUATORS - SetActuatorsFromCommands(commands); -#endif - SendCscFromActuators(); - -} - -static void csc_main_event( void ) -{ - csc_can_event(); - xsens_event_task(); - DatalinkEvent(); -} - -int main( void ) { - csc_main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) - csc_main_periodic(); - csc_main_event(); - } - return 0; -} diff --git a/sw/airborne/csc/csc_baro.c b/sw/airborne/csc/csc_baro.c deleted file mode 100644 index 08c7edbe21..0000000000 --- a/sw/airborne/csc/csc_baro.c +++ /dev/null @@ -1,169 +0,0 @@ -#include "std.h" -#include "mcu.h" -#include "mcu_periph/sys_time.h" -#include "led.h" -#include "interrupt_hw.h" - -#include "mcu_periph/uart.h" -#include "messages.h" -#include "subsystems/datalink/downlink.h" -#include "csc_booz2_ins.h" -#include "csc_ap_link.h" - -#include "mcu_periph/spi_arch.h" - -#include "csc_baro.h" - -uint8_t baro_scp_status; -uint32_t baro_scp_pressure; -uint16_t baro_scp_temperature; -float baro_scp_alt; -bool_t baro_scp_available; - -static void baro_scp_start_high_res_measurement(void); -static void spi1_isr(void); - -void baro_scp_periodic(void) { - if (cpu_time_sec > 1) { - if (baro_scp_status == STA_UNINIT) { - baro_scp_start_high_res_measurement(); - baro_scp_status = STA_INITIALISING; - } else { - spi1_isr(); - } - } -} - -#define SS_PIN 30 -#define SS_IODIR IO0DIR -#define SS_IOSET IO0SET -#define SS_IOCLR IO0CLR - -#define ScpSelect() SetBit(SS_IOCLR,SS_PIN) -#define ScpUnselect() SetBit(SS_IOSET,SS_PIN) - -// DRDY1 aka P0.15 -#define DRDY1_PIN 15 -#define DRDY1_IODIR IO0DIR -#define DRDY1_IOPIN IO0PIN -#define DRDY1_PINSEL() PINSEL0 = (PINSEL0 & ~(3 << 30)) | (0 << 30) - -#define SSP1_SCK_PINSEL() (PINSEL1 = (PINSEL1 & ~(3 << 2)) | (2 << 2)) -#define SSP1_MISO_PINSEL() (PINSEL1 = (PINSEL1 & ~(3 << 4)) | (2 << 4)) -#define SSP1_MOSI_PINSEL() (PINSEL1 = (PINSEL1 & ~(3 << 6)) | (2 << 6)) -#define SSP1_SSEL_PINSEL() (PINSEL1 = (PINSEL1 & ~(3 << 8)) | (2 << 8)) - -static uint8_t baro_scp_read_reg_8(uint8_t regno) -{ - uint8_t value; - uint8_t foo0 __attribute__ ((unused)); - - ScpSelect(); - sys_time_usleep(20); - S1SPDR = regno << 2; - while(~S1SPSR & _BV(7)); - foo0 = S1SPDR; - - S1SPDR = 0; - while(~S1SPSR & _BV(7)); - value = S1SPDR; - sys_time_usleep(10); - ScpUnselect(); - - return value; -} - -static void baro_scp_write_reg_8(uint8_t regno, uint8_t value) -{ - uint8_t foo0 __attribute__ ((unused)); - uint8_t foo1 __attribute__ ((unused)); - - ScpSelect(); - - foo0 = S1SPSR; - foo0 = S1SPDR; - - sys_time_usleep(20); - S1SPDR = (regno << 2) | (1 << 1); - while(~S1SPSR & _BV(7)); - foo0 = S1SPDR; - - S1SPDR = value; - foo1 = S1SPDR; - while(~S1SPSR & _BV(7)); - - sys_time_usleep(10); - ScpUnselect(); -} - -static uint16_t baro_scp_read_reg_16(uint8_t regno) -{ - uint8_t lsb, msb; - uint8_t foo0 __attribute__ ((unused)); - - ScpSelect(); - sys_time_usleep(20); - S1SPDR = regno << 2; - while(~S1SPSR & _BV(7)); - foo0 = S1SPDR; - - S1SPDR = 0; - while(~S1SPSR & _BV(7)); - msb = S1SPDR; - - S1SPDR = 0; - while(~S1SPSR & _BV(7)); - lsb = S1SPDR; - - sys_time_usleep(10); - ScpUnselect(); - - - return (uint16_t) ((msb << 8) | lsb); -} - -void baro_scp_init( void ) -{ - - SSP1_SCK_PINSEL(); - SSP1_MISO_PINSEL(); - SSP1_MOSI_PINSEL(); - SSP1_SSEL_PINSEL(); - DRDY1_PINSEL(); - - S1SPCR = _BV(3) | _BV(4) | _BV(5); - S1SPCCR = 0x80; - - baro_scp_status = STA_UNINIT; -} - -void spi1_isr() -{ - uint32_t msb; - uint16_t lsb; - - if (bit_is_set(DRDY1_IOPIN, DRDY1_PIN)) { - sys_time_usleep(10); - baro_scp_temperature = baro_scp_read_reg_16(0x21); - msb = baro_scp_read_reg_8(0x1F); - lsb = baro_scp_read_reg_16(0x20); - - //if (baro_scp_temperature & 0x2000) { - // baro_scp_temperature |= 0xC000; - //} - baro_scp_temperature = baro_scp_temperature & (0xFFFF >> 2); - baro_scp_temperature *= 5; - - baro_scp_pressure = lsb; - baro_scp_pressure |= (msb << 16); - baro_scp_pressure *= 25; - - //baro_scp_alt = 9000 - baro_scp_pressure * 0.00084366; - baro_scp_status = STA_VALID; - } -} - -/* write 0x0A to 0x03 */ -static void baro_scp_start_high_res_measurement(void) { - baro_scp_write_reg_8(0x3, 0xA); -} diff --git a/sw/airborne/csc/csc_main.c b/sw/airborne/csc/csc_main.c deleted file mode 100644 index 40f7db23ec..0000000000 --- a/sw/airborne/csc/csc_main.c +++ /dev/null @@ -1,228 +0,0 @@ -/* - * $Id: booz2_main.c 3049 2009-02-24 16:51:25Z poine $ - * - * Copyright (C) 2008 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 - -#include "csc_main.h" - -#include "std.h" - -#include "mcu.h" -#include "mcu_periph/sys_time.h" -#include "led.h" -#include "interrupt_hw.h" -#include "mcu_periph/uart.h" -#include "csc_telemetry.h" -#include "generated/periodic.h" -#include "subsystems/datalink/downlink.h" -#include "i2c.h" - -#include "csc_servos.h" -#include "csc_throttle.h" -#include "csc_adc.h" -#include "csc_rc_spektrum.h" -#include "csc_msg_def.h" - -#include "csc_can.h" -#include "csc_ap_link.h" -static inline void on_servo_cmd(struct CscServoCmd *cmd); -static inline void on_motor_cmd(struct CscMotorMsg *msg); - -#define SERVO_TIMEOUT (CPU_TICKS_OF_SEC(0.1) / PERIODIC_TASK_PERIOD) -#define CSC_STATUS_TIMEOUT (CPU_TICKS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD) - -static uint32_t servo_cmd_timeout = 0; -static uint32_t can_msg_count = 0; - -// gps stuff stolen from antoine's code -#include "booz/booz2_gps.h" -#include "math/pprz_geodetic_int.h" - -struct LtpDef_i booz_ins_ltp_def; - bool_t booz_ins_ltp_initialised; -struct NedCoor_i booz_ins_gps_pos_cm_ned; -struct NedCoor_i booz_ins_gps_speed_cm_s_ned; - -static void csc_main_init( void ) { - - mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); - led_init(); - -#ifdef USE_UART0 - Uart0Init(); -#endif -#ifdef USE_UART1 - Uart1Init(); -#endif - -#ifdef SPEKTRUM_LINK - spektrum_init(); -#endif - - -#if USE_GPS - booz2_gps_init(); -#endif - - csc_ap_link_init(); - csc_ap_link_set_servo_cmd_cb(on_servo_cmd); - csc_ap_link_set_motor_cmd_cb(on_motor_cmd); - - csc_adc_init(); - - #ifdef USE_I2C0 - i2c_init(); - #endif - // be sure to call servos_init after uart1 init since they are sharing pins - csc_servos_init(); -#ifdef USE_CSC_THROTTLE - csc_throttle_init(); -#endif - mcu_int_enable(); - - -} - - -static void csc_main_periodic( void ) { - static uint32_t zeros[4] = {0, 0, 0, 0}; - static uint32_t csc_loops = 0; - - #ifdef DOWNLINK - PeriodicSendAp(DefaultChannel); - #endif - - if (servo_cmd_timeout > SERVO_TIMEOUT) { - LED_OFF(CAN_LED); - csc_servos_set(zeros); - } else { - servo_cmd_timeout++; - } - - if ((++csc_loops % CSC_STATUS_TIMEOUT) == 0) { - csc_ap_link_send_status(csc_loops, can_msg_count); - } - if ((++csc_loops % CSC_STATUS_TIMEOUT) == 0) { - csc_adc_periodic(); - } - -} - -static inline void on_gps_event(void) { - if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) { - if (!booz_ins_ltp_initialised) { - ltp_def_from_ecef_i(&booz_ins_ltp_def, &booz_gps_state.ecef_pos); - booz_ins_ltp_initialised = TRUE; - } - ned_of_ecef_point_i(&booz_ins_gps_pos_cm_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_pos); - ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_vel); - } - - struct CscGPSFixMsg fix_msg; - struct CscGPSAccMsg acc_msg; - struct CscGPSPosMsg pos_msg; - - fix_msg.gps_fix = (booz_gps_state.fix == BOOZ2_GPS_FIX_3D); - fix_msg.vx = booz_ins_gps_speed_cm_s_ned.x; - fix_msg.vy = booz_ins_gps_speed_cm_s_ned.y; - fix_msg.vz = booz_ins_gps_speed_cm_s_ned.z; - csc_ap_send_msg(CSC_GPS_FIX_ID, (const uint8_t *) &fix_msg, sizeof(fix_msg)); - - acc_msg.pacc = booz_gps_state.pacc; - acc_msg.sacc = booz_gps_state.sacc; - csc_ap_send_msg(CSC_GPS_ACC_ID, (const uint8_t *) &acc_msg, sizeof(acc_msg)); - - pos_msg.val = booz_ins_gps_pos_cm_ned.x; - pos_msg.axis = CSC_GPS_AXIS_IDX_X; - csc_ap_send_msg(CSC_GPS_POS_ID, (const uint8_t *) &pos_msg, sizeof(pos_msg)); - - pos_msg.val = booz_ins_gps_pos_cm_ned.y; - pos_msg.axis = CSC_GPS_AXIS_IDX_Y; - csc_ap_send_msg(CSC_GPS_POS_ID, (const uint8_t *) &pos_msg, sizeof(pos_msg)); - - pos_msg.val = booz_ins_gps_pos_cm_ned.z; - pos_msg.axis = CSC_GPS_AXIS_IDX_Z; - csc_ap_send_msg(CSC_GPS_POS_ID, (const uint8_t *) &pos_msg, sizeof(pos_msg)); - -} - -static void csc_main_event( void ) { - - csc_can_event(); -#ifdef USE_CSC_THROTTLE - csc_throttle_event_task(); -#endif -#ifdef SPEKTRUM_LINK - spektrum_event_task(); -#endif -#if USE_GPS - Booz2GpsEvent(on_gps_event); -#endif -} - - -#define MIN_SERVO CPU_TICKS_OF_USEC(1000) -#define MAX_SERVO CPU_TICKS_OF_USEC(2000) - -static inline void on_servo_cmd(struct CscServoCmd *cmd) -{ - - uint32_t servos_checked[4]; - uint32_t i; - for (i=0; i<4; i++) - servos_checked[i] = cmd->servos[i]; - // servos_checked[i] = Chop(servos[i],MIN_SERVO, MAX_SERVO); - csc_servos_set(servos_checked); - - servo_cmd_timeout = 0; - ++can_msg_count; - - // DOWNLINK_SEND_CSC_CAN_MSG(&can1_rx_msg.frame, &can1_rx_msg.id, - // &can1_rx_msg.dat_a, &can1_rx_msg.dat_b); - // DOWNLINK_SEND_ADC_GENERIC(&servos[0], &servos[1]); - -} - - -static inline void on_motor_cmd(struct CscMotorMsg *msg) -{ - // always send to throttle_id zero, only one motorcontrol per csc board - const static uint8_t throttle_id = 0; - - #ifdef USE_CSC_THROTTLE - csc_throttle_send_msg(throttle_id, msg->cmd_id, msg->arg1, msg->arg2); - #endif -} - -int main( void ) { - csc_main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) - csc_main_periodic(); - csc_main_event(); - } - return 0; -} - diff --git a/sw/airborne/csc/csc_protocol.c b/sw/airborne/csc/csc_protocol.c deleted file mode 100644 index 5acc9fee53..0000000000 --- a/sw/airborne/csc/csc_protocol.c +++ /dev/null @@ -1,190 +0,0 @@ -/* - * $Id:$ - * - * Copyright (C) 2010 Piotr Esden-Tempski - * - * 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 -#include - -#include "csc_protocol.h" -#include "mcu_periph/can.h" -#include "csc_msg_def.h" - -// #include "mcu.h" -// #include "mcu_periph/sys_time.h" -// #include "subsystems/datalink/downlink.h" - -#define CSCP_QUEUE_LEN 8 - -struct cscp_msg { - uint8_t id; - uint8_t len; - uint8_t data[8]; -}; - -struct cscp_msg_queue { - int head; - int tail; - int full; - int empty; - struct cscp_msg msgs[CSCP_QUEUE_LEN]; -} cscp_msg_queue; - -/* - * This handle is being used internally in the protocol driver - * data points to user supplied memory (user allocated struct) - * this way we make sure that the user allocates only the amount - * of memory necessary and we don't have to mess around with malloc - */ -struct cscp_callback_handle { - cscp_callback_t callback; - void *data; -}; - -struct cscp_callback_handle cscp_callback_handles[CSC_ID_COUNT]; - -void cscp_can_rx_callback(uint32_t id, uint8_t *buf, int len); -void cscp_queue_init(void); -int cscp_enqueue(uint32_t msg_id, uint8_t *buf, int len); -int cscp_peek_msg_id(void); -int cscp_dequeue(uint8_t *buf); -void cscp_callback_init(void); - -void cscp_init(void) -{ - can_init(cscp_can_rx_callback); - cscp_queue_init(); - cscp_callback_init(); -} - -static inline uint32_t cscp_can_id(uint8_t client_id, uint8_t msg_id) -{ - return ((client_id & 0xF) << 7) | (msg_id & 0x7F); -} - -static inline uint32_t cscp_msg_id(uint32_t id) -{ - return (id & 0x7F); -} - -int cscp_transmit(uint32_t client_id, uint8_t msg_id, const uint8_t *buf, uint8_t len) -{ - uint32_t id = cscp_can_id(client_id, msg_id); - return can_transmit(id, buf, len); -} - -void cscp_can_rx_callback(uint32_t id, uint8_t *buf, int len) -{ - /* just store the incoming data in a buffer with very little processing */ - - /* TODO we should handle the return value of enqueue somehow, - * the return value tells us if the queue had enough space to - * store our message or not - */ - uint32_t msg_id = cscp_msg_id(id); - - cscp_enqueue(msg_id, buf, len); -} - -void cscp_event(void) -{ - /* this periodig is the main csc protocol event dispatcher */ - int msg_id = cscp_peek_msg_id(); - - if (msg_id == -1){ - return; - } - - if(cscp_callback_handles[msg_id].callback){ - cscp_dequeue(cscp_callback_handles[msg_id].data); - cscp_callback_handles[msg_id].callback(cscp_callback_handles[msg_id].data); - return; - } - -} - -void cscp_queue_init(void) -{ - cscp_msg_queue.head = 0; - cscp_msg_queue.tail = 0; - cscp_msg_queue.full = 0; - cscp_msg_queue.empty = 1; -} - -int cscp_enqueue(uint32_t msg_id, uint8_t *buf, int len) -{ - if (cscp_msg_queue.full) { - cscp_msg_queue.empty = 0; - return 1; - } - if(!cscp_callback_handles[msg_id].callback) { - return 2; - } - - cscp_msg_queue.msgs[cscp_msg_queue.tail].id = msg_id; - cscp_msg_queue.msgs[cscp_msg_queue.tail].len = len; - memcpy(cscp_msg_queue.msgs[cscp_msg_queue.tail].data, buf, len); - cscp_msg_queue.tail = (cscp_msg_queue.tail + 1) % CSCP_QUEUE_LEN; - cscp_msg_queue.empty = 0; - if (cscp_msg_queue.head == cscp_msg_queue.tail) { - cscp_msg_queue.full = 1; - } - - return 0; -} - -int cscp_peek_msg_id(void) -{ - if (cscp_msg_queue.empty) { - return -1; - } - return cscp_msg_queue.msgs[cscp_msg_queue.head].id; -} - -int cscp_dequeue(uint8_t *buf) -{ - if (cscp_msg_queue.empty) { - return 1; - } - - memcpy(buf, - cscp_msg_queue.msgs[cscp_msg_queue.head].data, - cscp_msg_queue.msgs[cscp_msg_queue.head].len); - cscp_msg_queue.head = (cscp_msg_queue.head + 1) % CSCP_QUEUE_LEN; - cscp_msg_queue.full = 0; - if (cscp_msg_queue.head == cscp_msg_queue.tail) { - cscp_msg_queue.empty = 1; - } - - return 0; -} - -void cscp_callback_init(void) -{ - memset(cscp_callback_handles, 0, sizeof(cscp_callback_handles)); -} - -void cscp_register_callback(uint32_t msg_id, cscp_callback_t callback, uint8_t *data) -{ - cscp_callback_handles[msg_id].callback = callback; - cscp_callback_handles[msg_id].data = data; -} diff --git a/sw/airborne/csc/csc_servos.c b/sw/airborne/csc/csc_servos.c deleted file mode 100644 index 9490b6749d..0000000000 --- a/sw/airborne/csc/csc_servos.c +++ /dev/null @@ -1,76 +0,0 @@ -#include "csc_servos.h" - -#include "LPC21xx.h" -#include "std.h" -#include "mcu_periph/sys_time.h" -#include "firmwares/rotorcraft/actuators.h" -#include "generated/airframe.h" -#include ACTUATORS - -#define CSC_SERVOS_NB 4 - -static uint32_t csc_servos_rng[] = {CPU_TICKS_OF_USEC(SERVO_S1_MAX-SERVO_S1_MIN), - CPU_TICKS_OF_USEC(SERVO_S2_MAX-SERVO_S2_MIN), - CPU_TICKS_OF_USEC(SERVO_S3_MAX-SERVO_S3_MIN), - CPU_TICKS_OF_USEC(SERVO_S4_MAX-SERVO_S4_MIN)}; -static uint32_t csc_servos_min[] = {CPU_TICKS_OF_USEC(SERVO_S1_MIN), - CPU_TICKS_OF_USEC(SERVO_S2_MIN), - CPU_TICKS_OF_USEC(SERVO_S3_MIN), - CPU_TICKS_OF_USEC(SERVO_S4_MIN)}; - - -void csc_servos_init(void) -{ - actuators_init(); -} - -/* val == 0 literally mean off, otherwise, val 1-255 are continuous angle */ -void csc_servo_normalized_set(uint8_t id, uint16_t val) -{ - if(id > 3) return; - if(val == 0) csc_servo_set(id,0); - else{ - uint32_t ticks = csc_servos_rng[id]*(val-1); - - csc_servo_set(id,ticks/((1<<16)-2) + csc_servos_min[id]); - } -} - - -void csc_servos_commit() -{ - ActuatorsCommit(); -} - -void csc_servo_set(uint8_t id, uint32_t val) -{ - if(id > 3) return; - -#ifdef CSC_MOTORS_I2C - Actuator(id) = val; -#else - switch(id){ - case 0: Actuator(0) = val; break; - case 1: Actuator(5) = val; break; - case 2: Actuator(4) = val; break; - case 3: Actuator(3) = val; break; - } -#endif -} - -void csc_servos_set(int32_t* val) -{ -#ifdef CSC_MOTORS_I2C - Actuator(0) = val[0]; - Actuator(1) = val[1]; - Actuator(2) = val[2]; - Actuator(3) = val[3]; -#else - Actuator(0) = val[0]; - Actuator(5) = val[1]; - Actuator(4) = val[2]; - Actuator(3) = val[3]; -#endif - - csc_servos_commit(); -} diff --git a/sw/airborne/csc/mercury_csc_main.c b/sw/airborne/csc/mercury_csc_main.c deleted file mode 100644 index ce9c725186..0000000000 --- a/sw/airborne/csc/mercury_csc_main.c +++ /dev/null @@ -1,262 +0,0 @@ -/* - * $Id: booz2_main.c 3049 2009-02-24 16:51:25Z poine $ - * - * Copyright (C) 2008 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 - -#include "csc_main.h" - -#include "std.h" - -#include "mcu.h" -#include "mcu_periph/sys_time.h" -#include "led.h" - -#include "csc_vane.h" - -#ifdef USE_BUSS_TWI_BLMC_MOTOR -#include "buss_twi_blmc_hw.h" -#endif -#include "i2c.h" - -#include "csc_servos.h" - -#include "airspeed.h" -#include "baro_ets.h" -#include "airspeed_ets.h" -// #include "ams5812.h" - -#include "interrupt_hw.h" -#include "mcu_periph/uart.h" -#include "csc_telemetry.h" - -#include "generated/periodic.h" -#include "subsystems/datalink/downlink.h" - -#include "pwm_input.h" -#include "csc_airspeed.h" -#include "csc_baro.h" -#include "csc_bat_monitor.h" - -#include "csc_adc.h" - -#include "csc_rc_spektrum.h" - -#include "csc_can.h" -#include "csc_ap_link.h" - -static inline void on_servo_cmd(struct CscServoCmd *cmd); -static inline void on_motor_cmd(struct CscMotorMsg *msg); -static inline void on_prop_cmd(struct CscPropCmd *msg, int idx); - -#define SERVO_TIMEOUT (CPU_TICKS_OF_SEC(0.1) / PERIODIC_TASK_PERIOD) -#define CSC_STATUS_TIMEOUT (CPU_TICKS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD) -#define AIRSPEED_TIMEOUT (CPU_TICKS_OF_SEC(0.01) / PERIODIC_TASK_PERIOD) - - -static uint32_t servo_cmd_timeout = 0; -static uint32_t can_msg_count = 0; - - -static void csc_main_init( void ) { - - hw_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); - led_init(); - - actuators_init(); - - -#ifdef USE_UART0 - Uart0Init(); -#endif - -#ifdef USE_UART1 - Uart1Init(); -#endif - -#ifdef SPEKTRUM_LINK - spektrum_init(); -#endif - -#ifdef USE_PWM_INPUT - pwm_input_init(); -#endif - - -// be sure to call servos_init after uart1 init since they are sharing pins - csc_servos_init(); - -#ifdef USE_VANE_SENSOR -#ifndef USE_PWM_INPUT - pwm_input_init(); -#endif -#endif - - csc_ap_link_init(); - csc_ap_link_set_servo_cmd_cb(on_servo_cmd); - csc_ap_link_set_motor_cmd_cb(on_motor_cmd); - csc_ap_link_set_prop_cmd_cb(on_prop_cmd); - -#ifdef ADC - csc_adc_init(); -#endif - - -#ifdef USE_I2C0 - i2c0_init(); -#endif - -#ifdef USE_BUSS_TWI_BLMC_MOTOR - motors_init(); -#endif - -#if USE_AIRSPEED - airspeed_init(); -#endif -#ifdef USE_AMS5812 - csc_ams5812_init(); -#endif -#if USE_BARO_SCP - baro_scp_init(); -#endif - -#ifdef USE_BAT_MONITOR - csc_bat_monitor_init(); -#endif - - int_enable(); - -} - -static void csc_main_periodic( void ) -{ - static uint32_t zeros[4] = {0, 0, 0, 0}; - static uint32_t csc_loops = 0; - -#ifdef DOWNLINK - PeriodicSendAp(DefaultChannel); -#endif - -#ifdef USE_VANE_SENSOR - csc_vane_periodic(); -#endif - - if (servo_cmd_timeout > SERVO_TIMEOUT) { - csc_servos_set(zeros); - } else { - servo_cmd_timeout++; - } - - if ((++csc_loops % CSC_STATUS_TIMEOUT) == 0) { - csc_ap_link_send_status(csc_loops, can_msg_count); - } - - csc_adc_periodic(); - - if ((csc_loops % AIRSPEED_TIMEOUT) == 0) { - } else if ((csc_loops % AIRSPEED_TIMEOUT) == 1) { -#if USE_BARO_ETS - baro_ets_periodic(); -#endif -#if USE_AIRSPEED - csc_airspeed_periodic(); -#endif -#ifdef USE_AMS5812 - csc_ams5812_periodic(); - csc_ap_link_send_pressure(ams5812_pressure[0], ams5812_pressure[1]); -#endif - } - -#if USE_AIRSPEED - airspeed_update(); -#endif - -#if USE_BARO_SCP - baro_scp_periodic(); - csc_ap_link_send_baro(baro_scp_pressure, baro_scp_temperature, baro_scp_status); -#endif - -#ifdef USE_BAT_MONITOR - csc_bat_monitor_periodic(); -#endif - -} - -static void csc_main_event( void ) { - csc_can_event(); - -#ifdef USE_BUSS_TWI_BLMC_MOTOR - motors_event(); -#endif - -#ifdef SPEKTRUM_LINK - spektrum_event_task(); -#endif -} - -#define MIN_SERVO CPU_TICKS_OF_USEC(1000) -#define MAX_SERVO CPU_TICKS_OF_USEC(2000) - -#ifdef USE_BUSS_TWI_BLMC_MOTOR -static void on_prop_cmd(struct CscPropCmd *cmd, int idx) -{ - for(uint8_t i = 0; i < 4; i++) - motors_set_motor(i + idx * 4, cmd->speeds[i]); - - motors_commit(idx == 1); - - ++can_msg_count; -} -#else -static void on_prop_cmd(struct CscPropCmd *cmd, int idx) {} -#endif - -static void on_servo_cmd(struct CscServoCmd *cmd) -{ - uint16_t* servos = (uint16_t*)(cmd); - uint8_t i; - - // uint32_t servos_checked[4]; - for(i = 0; i < 4; i++) { - csc_servo_normalized_set(i,servos[i]); - } - csc_servos_commit(); - servo_cmd_timeout = 0; - ++can_msg_count; -} - -static void on_motor_cmd(struct CscMotorMsg *msg) {} - -int main( void ) { - csc_main_init(); -// Uart0PrintString("Hello"); - while(1) { - if (sys_time_check_and_ack_timer(0)) { - csc_main_periodic(); - } - csc_main_event(); - } - return 0; -} - diff --git a/sw/airborne/csc/mercury_main.c b/sw/airborne/csc/mercury_main.c deleted file mode 100644 index 1ce22fb4b5..0000000000 --- a/sw/airborne/csc/mercury_main.c +++ /dev/null @@ -1,171 +0,0 @@ -/* - * $Id: booz2_main.c 3049 2009-02-24 16:51:25Z poine $ - * - * Copyright (C) 2008 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 - -#include "csc_main.h" - -#include "std.h" - -#include "mcu.h" -#include "mcu_periph/sys_time.h" -#include "led.h" -#include "interrupt_hw.h" -#include "mcu_periph/uart.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic.h" -#include "generated/airframe.h" -#include "commands.h" - -#include "csc_msg_def.h" -#include ACTUATORS -#include "subsystems/imu.h" -#include "booz/ahrs/ahrs_aligner.h" -#include "booz/ahrs.h" -#include "mercury_xsens.h" -#include "csc_telemetry.h" -#include "csc_adc.h" -#include "mercury_ap.h" -#include "booz/booz2_autopilot.h" -#include "booz/guidance/booz2_guidance_v.h" -#include "csc_can.h" -#include "csc_baro.h" -#include "subsystems/radio_control.h" - -#include "booz/stabilization/stabilization_attitude.h" - -extern uint8_t vsupply; - - -#define CSC_STATUS_TIMEOUT (CPU_TICKS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD) - -#define PPRZ_MODE_MOTORS_OFF 0 -#define PPRZ_MODE_MOTORS_ON 1 -#define PPRZ_MODE_IN_FLIGHT 2 - -uint8_t pprz_mode = PPRZ_MODE_MOTORS_OFF; -static uint16_t cpu_time = 0; - -static inline void csc_main_init( void ); -static inline void csc_main_periodic( void ); -static inline void csc_main_event( void ); - -int main( void ) { - csc_main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) - csc_main_periodic(); - csc_main_event(); - } - return 0; -} - -static void nop(struct CscCanMsg *msg) -{ - -} - -static void on_rc_cmd(struct CscRCMsg *msg) -{ - uint16_t aux2_flag; - - radio_control.values[RADIO_ROLL] = CSC_RC_SCALE*(msg->right_stick_horizontal - CSC_RC_OFFSET); - radio_control.values[RADIO_PITCH] = -CSC_RC_SCALE*(msg->right_stick_vertical - CSC_RC_OFFSET); - radio_control.values[RADIO_YAW] = CSC_RC_SCALE*((msg->left_stick_horizontal_and_aux2 & ~(3 << 13)) - CSC_RC_OFFSET); - pprz_mode = (msg->left_stick_vertical_and_flap_mix & (3 << 13)) >> 13; - aux2_flag = (msg->left_stick_horizontal_and_aux2 >> 13) & 0x1; - radio_control.values[RADIO_MODE2] = (aux2_flag == 0) ? -7000 : ( (aux2_flag == 1) ? 0 : 7000); - radio_control.values[RADIO_MODE] = (pprz_mode == 0) ? -7000 : ( (pprz_mode == 1) ? 0 : 7000); - radio_control.values[RADIO_THROTTLE] = -CSC_RC_SCALE*((msg->left_stick_vertical_and_flap_mix & ~(3 << 13)) - CSC_RC_OFFSET); - - radio_control.time_since_last_frame = 0; - radio_control.status = RC_OK; -} - -static inline void csc_main_init( void ) { - - mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); - led_init(); - - Uart0Init(); - Uart1Init(); - - imu_init(); - - ahrs_aligner_init(); - ahrs_init(); - - xsens_init(); - - stabilization_attitude_init(); - booz2_guidance_v_init(); - booz_ins_init(); - - csc_adc_init(); - - baro_scp_init(); - - csc_ap_link_init(); - csc_ap_link_set_servo_cmd_cb(&nop); - csc_ap_link_set_motor_cmd_cb(&nop); - csc_ap_link_set_rc_cmd_cb(on_rc_cmd); - actuators_init(); - - props_init(); - - csc_ap_init(); - mcu_int_enable(); - - stabilization_attitude_enter(); -} - - -static inline void csc_main_periodic( void ) -{ - static uint32_t csc_loops = 0; - - PeriodicSendAp(); - radio_control_periodic(); - - cpu_time++; - - if ((++csc_loops % CSC_STATUS_TIMEOUT) == 0) { - csc_adc_periodic(); - } - xsens_periodic_task(); - - baro_scp_periodic(); - - csc_ap_periodic(pprz_mode == PPRZ_MODE_IN_FLIGHT, - !(pprz_mode > PPRZ_MODE_MOTORS_OFF && ahrs_aligner.status == AHRS_ALIGNER_LOCKED)); - -} - -static inline void csc_main_event( void ) -{ - csc_can_event(); - xsens_event_task(); - DatalinkEvent(); -} diff --git a/sw/airborne/csc/ppm_bridge_main.c b/sw/airborne/csc/ppm_bridge_main.c deleted file mode 100644 index 1cd8521013..0000000000 --- a/sw/airborne/csc/ppm_bridge_main.c +++ /dev/null @@ -1,131 +0,0 @@ -/* - * $Id: booz2_main.c 3049 2009-02-24 16:51:25Z poine $ - * - * Copyright (C) 2008 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 - -#include "csc_main.h" - -#include "std.h" - -#include "mcu.h" -#include "mcu_periph/sys_time.h" -#include "led.h" -#include "interrupt_hw.h" -#include "mcu_periph/uart.h" -#include "subsystems/datalink/downlink.h" -#include "generated/periodic.h" -#include "generated/airframe.h" -#include "commands.h" -#include "subsystems/radio_control.h" - -#include "csc_telemetry.h" -#include "led.h" - -#include "subsystems/datalink/pprz_transport.h" - -#define RC_PROTOCOL_SYNC 13999 - -extern uint8_t vsupply; - -static uint16_t cpu_time = 0; - -static void csc_main_init( void ) { - - mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); - led_init(); - - Uart0Init(); - Uart1Init(); - - ppm_init(); - - // Configure P0.21 as GPIO, output and then pull high as we use it to drive ppm input transistor - PINSEL1 = PINSEL1 & ~(0x3 << 10); - IO0DIR = IO0DIR | (0x1 << 21); - IO0PIN = IO0DIR | (0x1 << 21); - - mcu_int_enable(); - -} - - -static void csc_main_periodic( void ) -{ - PeriodicSendAp(DefaultChannel); - radio_control_periodic_task(); - - cpu_time++; -} - -static void send_short( int16_t s ) -{ - Uart1Transmit(s >> 8); - Uart1Transmit(s & 0xFF); -} - -static void send_channel ( int16_t c ) -{ - send_short(c); - send_short(~c); -} - -static void on_rc_event( void ) -{ - -#ifdef SWAP_STICKS - int temp; - temp = rc_values[RADIO_YAW]; - rc_values[RADIO_YAW] = rc_values[RADIO_ROLL]; - rc_values[RADIO_ROLL] = temp; -#endif - - // 7 channels - // integers -9600 to +9600 - - // sync bytes - send_channel(RC_PROTOCOL_SYNC); - - for (int i = 0; i < RADIO_CTL_NB; i++) { - send_channel(rc_values[i]); - } - - LED_TOGGLE(3); -} - -static void csc_main_event( void ) -{ - RadioControlEventCheckAndHandle(on_rc_event); - DatalinkEvent(); -} - -int main( void ) { - csc_main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) - csc_main_periodic(); - csc_main_event(); - } - return 0; -} diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c index 1951252929..196dc528fe 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c @@ -43,7 +43,9 @@ #endif #define Actuator(_x) actuators_pwm_values[_x] +#ifndef ChopServo #define ChopServo(x,a,b) Chop(x, a, b) +#endif #define ActuatorsCommit actuators_pwm_commit int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index b2eda139d3..ad9bc6f23f 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot.h @@ -45,6 +45,7 @@ #define AP_MODE_HOVER_CLIMB 10 #define AP_MODE_HOVER_Z_HOLD 11 #define AP_MODE_NAV 12 +#define AP_MODE_RC_DIRECT 13 // Safety Pilot Direct Commands for helicopter direct control: appropriately chosen as mode "13" extern uint8_t autopilot_mode; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 5bb1c2dfa8..4c7a1eefd4 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -146,12 +146,12 @@ void guidance_h_read_rc(bool_t in_flight) { break; case GUIDANCE_H_MODE_HOVER: - STABILIZATION_ATTITUDE_READ_RC(guidance_h_rc_sp, in_flight); + stabilization_attitude_read_rc_ref(&guidance_h_rc_sp, in_flight); break; case GUIDANCE_H_MODE_NAV: if (radio_control.status == RC_OK) { - STABILIZATION_ATTITUDE_READ_RC(guidance_h_rc_sp, in_flight); + stabilization_attitude_read_rc_ref(&guidance_h_rc_sp, in_flight); guidance_h_rc_sp.psi = 0; } else { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c similarity index 62% rename from sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.c rename to sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c index 435f832b50..10b3029f05 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.c @@ -4,7 +4,7 @@ * */ -/** \file quat_setpoint.c +/** \file quat_setpoint_int.c * \brief Quaternion setpoint generation * */ @@ -12,7 +12,7 @@ #include "subsystems/ahrs.h" #include "stabilization/stabilization_attitude_ref_quat_int.h" -#include "stabilization/quat_setpoint.h" +#include "stabilization/quat_setpoint_int.h" #include "stabilization.h" #include "messages.h" @@ -55,60 +55,7 @@ static void reset_sp_quat(int32_t _psi, int32_t _theta, struct Int32Quat *initia QUAT_COPY(stab_att_sp_quat, pitch_rotated_quat2); } -static void update_sp_quat_from_eulers(void) { - struct Int32RMat sp_rmat; - -#ifdef STICKS_RMAT312 - INT32_RMAT_OF_EULERS_312(sp_rmat, stab_att_sp_euler); -#else - INT32_RMAT_OF_EULERS_321(sp_rmat, stab_att_sp_euler); -#endif - INT32_QUAT_OF_RMAT(stab_att_sp_quat, sp_rmat); - INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat); -} - -/* FIXME: what is up with this??? -void stabilization_attitude_read_rc_incremental(bool_t enable_alpha_vane, bool_t enable_beta_vane) -{ - pprz_t roll = radio_control.values[RADIO_ROLL]; - pprz_t pitch = radio_control.values[RADIO_PITCH]; - pprz_t yaw = radio_control.values[RADIO_YAW]; - struct Int32Quat prev_sp_quat, q_e2s, temp_quat; - - struct Int32RMat R_e2s; - struct Int32Rates sticks_w_bn_e, sticks_w_bn_s; - - QUAT_COPY(prev_sp_quat, stab_att_sp_quat); - - sticks_w_bn_e.p = APPLY_DEADBAND(roll, STABILIZATION_ATTITUDE_DEADBAND_A) * ROLL_COEF_H; - sticks_w_bn_e.q = APPLY_DEADBAND(pitch, STABILIZATION_ATTITUDE_DEADBAND_E) * PITCH_COEF_RATE; - sticks_w_bn_e.r = APPLY_DEADBAND(yaw, STABILIZATION_ATTITUDE_DEADBAND_R) * YAW_COEF_RATE; - - // Don't let the sticks command a theta rotation in vane mode - if (enable_alpha_vane) { - sticks_w_bn_e.q = 0; - } - - if (enable_beta_vane) { - sticks_w_bn_e.r = 0; - } - // q_e2s = q_sp (comp) q_b^(-1) - INT_QUAT_INV_COMP_NORM_SHORTEST(q_e2s, ahrs.ltp_to_body_quat, stab_att_sp_quat); - - INT_RMAT_OF_QUAT(R_e2s, q_e2s); - - INT_RMAT_RATEMULT(sticks_w_bn_s, R_e2s, sticks_w_bn_e); - - INT_QUAT_DIFFERENTIAL(temp_quat, sticks_w_bn_s, 1/RC_UPDATE_FREQ); - - INT32_QUAT_COMP_NORM_SHORTEST(stab_att_sp_quat, prev_sp_quat, temp_quat); - - // update euler setpoints for telemetry - INT32_EULERS_OF_QUAT(stab_att_sp_euler, stab_att_sp_quat); -} -*/ - -void stabilization_attitude_read_rc_absolute(struct Int32Eulers sp, bool_t in_flight) { +void stabilization_attitude_read_rc_absolute(bool_t in_flight) { // FIXME: wtf??? #ifdef AIRPLANE_STICKS @@ -147,7 +94,7 @@ void stabilization_attitude_read_rc_absolute(struct Int32Eulers sp, bool_t in_fl INT32_EULERS_OF_QUAT(stab_att_sp_euler, stab_att_sp_quat); } -void stabilization_attitude_sp_enter() +void stabilization_attitude_sp_enter(void) { // reset setpoint to "hover" reset_sp_quat(0., 0., &ahrs.ltp_to_body_quat); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.h b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.h similarity index 56% rename from sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.h rename to sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.h index fe8ddc2230..da4b8b08ac 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint_int.h @@ -13,9 +13,6 @@ void stabilization_attitude_sp_enter(void); -// FIXME: apparently unused... still needed?? -//void stabilization_attitude_read_rc_incremental(bool_t enable_alpha_vane, bool_t enable_beta_vane); - -void stabilization_attitude_read_rc_absolute(struct Int32Eulers sp, bool_t in_flight); +void stabilization_attitude_read_rc_absolute(bool_t in_flight); #endif diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h index f4b1259353..74e46759d1 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h @@ -28,8 +28,6 @@ #include STABILISATION_ATTITUDE_H extern void stabilization_attitude_init(void); extern void stabilization_attitude_read_rc(bool_t in_flight); -extern void stabilization_attitude_read_beta_vane(float beta); -extern void stabilization_attitude_read_alpha_vane(float alpha); extern void stabilization_attitude_enter(void); extern void stabilization_attitude_run(bool_t in_flight); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index 24569f0e78..4524917124 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -1,6 +1,4 @@ /* - * $Id$ - * * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. @@ -69,7 +67,7 @@ void stabilization_attitude_init(void) { void stabilization_attitude_read_rc(bool_t in_flight) { - STABILIZATION_ATTITUDE_READ_RC(stab_att_sp_euler, in_flight); + stabilization_attitude_read_rc_ref(&stab_att_sp_euler, in_flight); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index 4ca17813fc..ca4ad1535f 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -213,6 +213,7 @@ void stabilization_attitude_run(bool_t enable_integrator) { attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &ahrs_float.body_rate_d, &stabilization_att_sum_err_quat); + // FIXME: this is very dangerous! only works if this really includes all commands for (int i = COMMAND_ROLL; i <= COMMAND_YAW_SURFACE; i++) { stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i]; } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index a221633e5e..fc8942161a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -1,5 +1,4 @@ /* - * * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. @@ -20,12 +19,15 @@ * Boston, MA 02111-1307, USA. */ -/** \file stabilization_attitude_quat_int.c - * \brief Booz quaternion attitude stabilization +/** @file stabilization_attitude_quat_int.c + * Rotorcraft quaternion attitude stabilization */ #include "firmwares/rotorcraft/stabilization.h" -#include "firmwares/rotorcraft/stabilization/quat_setpoint.h" + +#if USE_SETPOINTS_WITH_TRANSITIONS +#include "firmwares/rotorcraft/stabilization/quat_setpoint_int.h" +#endif #include #include "math/pprz_algebra_float.h" @@ -46,46 +48,6 @@ struct Int32Eulers stabilization_att_sum_err; int32_t stabilization_att_fb_cmd[COMMANDS_NB]; int32_t stabilization_att_ff_cmd[COMMANDS_NB]; -//static int gain_idx = STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT; - -/* -static const float phi_pgain[] = STABILIZATION_ATTITUDE_PHI_PGAIN; -static const float theta_pgain[] = STABILIZATION_ATTITUDE_THETA_PGAIN; -static const float psi_pgain[] = STABILIZATION_ATTITUDE_PSI_PGAIN; - -static const float phi_dgain[] = STABILIZATION_ATTITUDE_PHI_DGAIN; -static const float theta_dgain[] = STABILIZATION_ATTITUDE_THETA_DGAIN; -static const float psi_dgain[] = STABILIZATION_ATTITUDE_PSI_DGAIN; - -static const float phi_igain[] = STABILIZATION_ATTITUDE_PHI_IGAIN; -static const float theta_igain[] = STABILIZATION_ATTITUDE_THETA_IGAIN; -static const float psi_igain[] = STABILIZATION_ATTITUDE_PSI_IGAIN; - -static const float phi_ddgain[] = STABILIZATION_ATTITUDE_PHI_DDGAIN; -static const float theta_ddgain[] = STABILIZATION_ATTITUDE_THETA_DDGAIN; -static const float psi_ddgain[] = STABILIZATION_ATTITUDE_PSI_DDGAIN; - -static const float phi_dgain_d[] = STABILIZATION_ATTITUDE_PHI_DGAIN_D; -static const float theta_dgain_d[] = STABILIZATION_ATTITUDE_THETA_DGAIN_D; -static const float psi_dgain_d[] = STABILIZATION_ATTITUDE_PSI_DGAIN_D; - -static const float phi_pgain_surface[] = STABILIZATION_ATTITUDE_PHI_PGAIN_SURFACE; -static const float theta_pgain_surface[] = STABILIZATION_ATTITUDE_THETA_PGAIN_SURFACE; -static const float psi_pgain_surface[] = STABILIZATION_ATTITUDE_PSI_PGAIN_SURFACE; - -static const float phi_dgain_surface[] = STABILIZATION_ATTITUDE_PHI_DGAIN_SURFACE; -static const float theta_dgain_surface[] = STABILIZATION_ATTITUDE_THETA_DGAIN_SURFACE; -static const float psi_dgain_surface[] = STABILIZATION_ATTITUDE_PSI_DGAIN_SURFACE; - -static const float phi_igain_surface[] = STABILIZATION_ATTITUDE_PHI_IGAIN_SURFACE; -static const float theta_igain_surface[] = STABILIZATION_ATTITUDE_THETA_IGAIN_SURFACE; -static const float psi_igain_surface[] = STABILIZATION_ATTITUDE_PSI_IGAIN_SURFACE; - -static const float phi_ddgain_surface[] = STABILIZATION_ATTITUDE_PHI_DDGAIN_SURFACE; -static const float theta_ddgain_surface[] = STABILIZATION_ATTITUDE_THETA_DDGAIN_SURFACE; -static const float psi_ddgain_surface[] = STABILIZATION_ATTITUDE_PSI_DDGAIN_SURFACE; -*/ - #define IERROR_SCALE 1024 #define GAIN_PRESCALER_FF 1 #define GAIN_PRESCALER_P 1 @@ -96,43 +58,21 @@ void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); - /* - for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) { - VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]); - VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]); - VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]); - VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]); - VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]); - VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]); - VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]); - VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]); - VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]); - } - */ - INT32_QUAT_ZERO( stabilization_att_sum_err_quat ); INT_EULERS_ZERO( stabilization_att_sum_err ); } - /* -void stabilization_attitude_gain_schedule(uint8_t idx) -{ - if (gain_idx >= STABILIZATION_ATTITUDE_GAIN_NB) { - // This could be bad -- Just say no. - return; - } - gain_idx = idx; - stabilization_attitude_ref_schedule(idx); -} - */ - void stabilization_attitude_enter(void) { +#if !USE_SETPOINTS_WITH_TRANSITIONS + /* reset psi setpoint to current psi angle */ + stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi; +#endif + stabilization_attitude_ref_enter(); - INT32_QUAT_ZERO( stabilization_att_sum_err_quat ); - //FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers ); - INT_EULERS_ZERO( stabilization_att_sum_err ); + INT32_QUAT_ZERO(stabilization_att_sum_err_quat); + INT_EULERS_ZERO(stabilization_att_sum_err); } #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) @@ -208,18 +148,29 @@ void stabilization_attitude_run(bool_t enable_integrator) { INT_EULERS_ZERO( stabilization_att_sum_err ); } + /* compute the feed forward command */ attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains, &stab_att_ref_accel); + /* compute the feed back command */ attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains, &att_err, &rate_err, &stabilization_att_sum_err_quat); - for (int i = COMMAND_ROLL; i <= COMMAND_YAW; i++) { - stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i]; - Bound(stabilization_cmd[i], -200, 200); - } + /* sum feedforward and feedback */ + stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL]; + stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH]; + stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW]; + + /* bound the result */ + Bound(stabilization_cmd[COMMAND_ROLL], -200, 200); + Bound(stabilization_cmd[COMMAND_PITCH], -200, 200); + Bound(stabilization_cmd[COMMAND_YAW], -200, 200); } void stabilization_attitude_read_rc(bool_t in_flight) { - STABILIZATION_ATTITUDE_READ_RC(stab_att_sp_euler, in_flight); +#if USE_SETPOINTS_WITH_TRANSITIONS + stabilization_attitude_read_rc_absolute(in_flight); +#else + stabilization_attitude_read_rc_setpoint(in_flight); +#endif } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c index 075fc7f67d..de71614943 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c @@ -23,8 +23,8 @@ #include "firmwares/rotorcraft/stabilization.h" -struct Int32Eulers stab_att_sp_euler; -struct Int32Eulers stab_att_ref_euler; +struct Int32Eulers stab_att_sp_euler; ///< with #REF_ANGLE_FRAC +struct Int32Eulers stab_att_ref_euler; ///< with #REF_ANGLE_FRAC struct Int32Rates stab_att_ref_rate; struct Int32Rates stab_att_ref_accel; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h index b511ea7162..66cc06fed0 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h @@ -26,59 +26,6 @@ #include "stabilization_attitude_ref_int.h" -#include "subsystems/radio_control.h" - - -#define REF_ACCEL_FRAC 12 -#define REF_RATE_FRAC 16 -#define REF_ANGLE_FRAC 20 -#define REF_ANGLE_PI BFP_OF_REAL(3.1415926535897932384626433832795029, REF_ANGLE_FRAC) -#define REF_ANGLE_TWO_PI BFP_OF_REAL(2.*3.1415926535897932384626433832795029, REF_ANGLE_FRAC) -#define ANGLE_REF_NORMALIZE(_a) { \ - while (_a > REF_ANGLE_PI) _a -= REF_ANGLE_TWO_PI; \ - while (_a < -REF_ANGLE_PI) _a += REF_ANGLE_TWO_PI; \ - } - - - -/* - * Radio Control - */ -#define SP_MAX_PHI ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI) -#define SP_MAX_THETA ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA) -#define SP_MAX_R ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R) - - - - - -#define RC_UPDATE_FREQ 40 - -#define YAW_DEADBAND_EXCEEDED() \ - (radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_DEADBAND_R || \ - radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R) - -#define STABILIZATION_ATTITUDE_READ_RC(_sp, _inflight) { \ - \ - _sp.phi = \ - ((int32_t)-radio_control.values[RADIO_ROLL] * (int32_t)SP_MAX_PHI / MAX_PPRZ) \ - << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ - _sp.theta = \ - ((int32_t) radio_control.values[RADIO_PITCH] * (int32_t)SP_MAX_THETA / MAX_PPRZ) \ - << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ - if (_inflight) { \ - if (YAW_DEADBAND_EXCEEDED()) { \ - _sp.psi += \ - ((int32_t)-radio_control.values[RADIO_YAW] * (int32_t)SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ) \ - << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ - ANGLE_REF_NORMALIZE(_sp.psi); \ - } \ - } \ - else { /* if not flying, use current yaw as setpoint */ \ - _sp.psi = (ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); \ - } \ - } - #define STABILIZATION_ATTITUDE_ADD_SP(_add_sp) { \ EULERS_ADD(stab_att_sp_euler,_add_sp); \ ANGLE_REF_NORMALIZE(stab_att_sp_euler.psi); \ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h index 36f31cdd97..fcdf56f0eb 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h @@ -20,8 +20,8 @@ * the Free Software Foundation, 59 Temple Place - Suite 330, * Boston, MA 02111-1307, USA. */ -#ifndef BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H -#define BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H +#ifndef STABILISATION_ATTITUDE_REF_FLOAT_H +#define STABILISATION_ATTITUDE_REF_FLOAT_H #include "generated/airframe.h" @@ -39,4 +39,4 @@ struct FloatRefModel { extern struct FloatRefModel stab_att_ref_model[]; -#endif /* BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H */ +#endif /* STABILISATION_ATTITUDE_REF_FLOAT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h index 25e38111fc..ef6d03ab7d 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h @@ -20,17 +20,25 @@ * the Free Software Foundation, 59 Temple Place - Suite 330, * Boston, MA 02111-1307, USA. */ -#ifndef BOOZ_STABILISATION_ATTITUDE_REF_INT_H -#define BOOZ_STABILISATION_ATTITUDE_REF_INT_H +#ifndef STABILISATION_ATTITUDE_REF_INT_H +#define STABILISATION_ATTITUDE_REF_INT_H -extern struct Int32Eulers stab_att_sp_vi_euler; /* vehicle interface */ -extern struct Int32Eulers stab_att_sp_rc_euler; /* radio control */ -extern struct Int32Eulers stab_att_sp_euler; /* sum of the above */ -extern struct Int32Quat stab_att_sp_quat; +#include "generated/airframe.h" +#include "math/pprz_algebra_int.h" + +// FIXME: move stabilization_attitude_read_rc_ref somewere else, so we don't need these includes here??? +#include "subsystems/radio_control.h" +#include "subsystems/ahrs.h" + +/* + * CAUTION! stabilization euler has the euler angles with REF_ANGLE_FRAC, but stab quat with INT32_ANGLE_FRAC + */ +extern struct Int32Eulers stab_att_sp_euler; +extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC extern struct Int32Eulers stab_att_ref_euler; -extern struct Int32Quat stab_att_ref_quat; -extern struct Int32Rates stab_att_ref_rate; -extern struct Int32Rates stab_att_ref_accel; +extern struct Int32Quat stab_att_ref_quat; ///< with #INT32_QUAT_FRAC +extern struct Int32Rates stab_att_ref_rate; ///< with #REF_RATE_FRAC +extern struct Int32Rates stab_att_ref_accel; ///< with #REF_ACCEL_FRAC struct Int32RefModel { struct Int32Rates omega; @@ -39,4 +47,47 @@ struct Int32RefModel { extern struct Int32RefModel stab_att_ref_model; -#endif /* BOOZ_STABILISATION_ATTITUDE_REF_INT_H */ +#define REF_ACCEL_FRAC 12 +#define REF_RATE_FRAC 16 +#define REF_ANGLE_FRAC 20 + +#define REF_ANGLE_PI BFP_OF_REAL(3.1415926535897932384626433832795029, REF_ANGLE_FRAC) +#define REF_ANGLE_TWO_PI BFP_OF_REAL(2.*3.1415926535897932384626433832795029, REF_ANGLE_FRAC) +#define ANGLE_REF_NORMALIZE(_a) { \ + while (_a > REF_ANGLE_PI) _a -= REF_ANGLE_TWO_PI; \ + while (_a < -REF_ANGLE_PI) _a += REF_ANGLE_TWO_PI; \ + } + +#define SP_MAX_PHI (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI) +#define SP_MAX_THETA (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA) +#define SP_MAX_R (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R) + + +#define RC_UPDATE_FREQ 40 + +#define YAW_DEADBAND_EXCEEDED() \ + (radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_DEADBAND_R || \ + radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R) + +static inline void stabilization_attitude_read_rc_ref(struct Int32Eulers *sp, bool_t in_flight) { + + sp->phi = ((int32_t)-radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ) + << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); + + sp->theta = ((int32_t) radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ) + << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); + + if (in_flight) { + if (YAW_DEADBAND_EXCEEDED()) { + sp->psi += ((int32_t)-radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ) + << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); + ANGLE_REF_NORMALIZE(sp->psi); + } + } + else { /* if not flying, use current yaw as setpoint */ + sp->psi = (ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); + } + +} + +#endif /* STABILISATION_ATTITUDE_REF_INT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c index cb28d79409..f31f758b4c 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c @@ -31,7 +31,6 @@ #include "subsystems/ahrs.h" #include "stabilization_attitude_ref_float.h" -#include "quat_setpoint.h" #define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_FLOAT_REF_MAX_PDOT #define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_FLOAT_REF_MAX_QDOT @@ -104,11 +103,7 @@ void stabilization_attitude_ref_enter() /* * Reference */ -#ifdef BOOZ_AP_PERIODIC_PRESCALE -#define DT_UPDATE ((float) BOOZ_AP_PERIODIC_PRESCALE / (float) PERIODIC_FREQ) -#else -#define DT_UPDATE (1./512.) -#endif +#define DT_UPDATE (1./PERIODIC_FREQUENCY) void stabilization_attitude_ref_update() { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h index bdd8fb5a79..20b22aba6c 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h @@ -1,6 +1,4 @@ /* - * $Id: stabilization_attitude_ref_traj_euler.h 3796 2009-07-25 00:01:02Z poine $ - * * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c index 17aaa88d7c..88484d7ca2 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c @@ -1,6 +1,4 @@ /* - * $Id$ - * * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. @@ -21,8 +19,8 @@ * Boston, MA 02111-1307, USA. */ -/** \file stabilization_attitude_ref_int.c - * \brief Booz attitude reference generation (quaternion int version) +/** @file stabilization_attitude_ref_int.c + * Rotorcraft attitude reference generation (quaternion int version) * */ @@ -31,7 +29,10 @@ #include "subsystems/ahrs.h" #include "stabilization_attitude_ref_int.h" -//#include "quat_setpoint.h" + +#if USE_SETPOINTS_WITH_TRANSITIONS +#include "firmwares/rotorcraft/stabilization/quat_setpoint_int.h" +#endif #define REF_ACCEL_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_PDOT, REF_ACCEL_FRAC) #define REF_ACCEL_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_QDOT, REF_ACCEL_FRAC) @@ -63,9 +64,9 @@ #define OMEGA_2_R BFP_OF_REAL((OMEGA_R*OMEGA_R), OMEGA_2_R_RES) -struct Int32Eulers stab_att_sp_euler; +struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC struct Int32Quat stab_att_sp_quat; -struct Int32Eulers stab_att_ref_euler; +struct Int32Eulers stab_att_ref_euler; ///< with #INT32_ANGLE_FRAC struct Int32Quat stab_att_ref_quat; struct Int32Rates stab_att_ref_rate; struct Int32Rates stab_att_ref_accel; @@ -75,14 +76,6 @@ struct Int32RefModel stab_att_ref_model = { {STABILIZATION_ATTITUDE_REF_ZETA_P, STABILIZATION_ATTITUDE_REF_ZETA_Q, STABILIZATION_ATTITUDE_REF_ZETA_R} }; -/* -static const float omega_p[] = STABILIZATION_ATTITUDE_REF_OMEGA_P; -static const float zeta_p[] = STABILIZATION_ATTITUDE_REF_ZETA_P; -static const float omega_q[] = STABILIZATION_ATTITUDE_REF_OMEGA_Q; -static const float zeta_q[] = STABILIZATION_ATTITUDE_REF_ZETA_Q; -static const float omega_r[] = STABILIZATION_ATTITUDE_REF_OMEGA_R; -static const float zeta_r[] = STABILIZATION_ATTITUDE_REF_ZETA_R; -*/ static void reset_psi_ref_from_body(void) { stab_att_ref_euler.psi = ahrs.ltp_to_body_euler.psi; @@ -90,26 +83,14 @@ static void reset_psi_ref_from_body(void) { stab_att_ref_accel.r = 0; } -static void update_ref_quat_from_eulers(void) { - struct Int32RMat ref_rmat; - -#ifdef STICKS_RMAT312 - INT32_RMAT_OF_EULERS_312(ref_rmat, stab_att_ref_euler); -#else - INT32_RMAT_OF_EULERS_321(ref_rmat, stab_att_ref_euler); -#endif - INT32_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat); - INT32_QUAT_WRAP_SHORTEST(stab_att_ref_quat); -} - void stabilization_attitude_ref_init(void) { INT_EULERS_ZERO(stab_att_sp_euler); - INT32_QUAT_ZERO( stab_att_sp_quat); + INT32_QUAT_ZERO(stab_att_sp_quat); INT_EULERS_ZERO(stab_att_ref_euler); - INT32_QUAT_ZERO( stab_att_ref_quat); - INT_RATES_ZERO( stab_att_ref_rate); - INT_RATES_ZERO( stab_att_ref_accel); + INT32_QUAT_ZERO(stab_att_ref_quat); + INT_RATES_ZERO(stab_att_ref_rate); + INT_RATES_ZERO(stab_att_ref_accel); /* for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) { @@ -123,23 +104,25 @@ void stabilization_attitude_ref_init(void) { void stabilization_attitude_ref_enter() { reset_psi_ref_from_body(); + +#if USE_SETPOINTS_WITH_TRANSITIONS stabilization_attitude_sp_enter(); memcpy(&stab_att_ref_quat, &stab_att_sp_quat, sizeof(struct Int32Quat)); +#else + update_quat_from_eulers(&stab_att_ref_quat, &stab_att_ref_euler); +#endif + + /* set reference rate and acceleration to zero */ memset(&stab_att_ref_accel, 0, sizeof(struct Int32Rates)); memset(&stab_att_ref_rate, 0, sizeof(struct Int32Rates)); - //update_ref_quat_from_eulers(); } /* * Reference */ -#define DT_UPDATE (1./512.) +#define DT_UPDATE (1./PERIODIC_FREQUENCY) #define F_UPDATE_RES 9 -#include "messages.h" -#include "mcu_periph/uart.h" -#include "subsystems/datalink/downlink.h" - void stabilization_attitude_ref_update() { /* integrate reference attitude */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h index c307fcab6e..aa1124e674 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h @@ -22,26 +22,14 @@ #define STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H #include "firmwares/rotorcraft/stabilization.h" -#include "firmwares/rotorcraft/stabilization/quat_setpoint.h" #include "subsystems/radio_control.h" #include "math/pprz_algebra_float.h" #include "stabilization_attitude_ref_int.h" -#define REF_ACCEL_FRAC 12 -#define REF_RATE_FRAC 16 -#define REF_ANGLE_FRAC 20 +#include "subsystems/ahrs.h" -#define REF_ANGLE_PI BFP_OF_REAL(3.1415926535897932384626433832795029, REF_ANGLE_FRAC) -#define REF_ANGLE_TWO_PI BFP_OF_REAL(2.*3.1415926535897932384626433832795029, REF_ANGLE_FRAC) -#define ANGLE_REF_NORMALIZE(_a) { \ - while (_a > REF_ANGLE_PI) _a -= REF_ANGLE_TWO_PI; \ - while (_a < -REF_ANGLE_PI) _a += REF_ANGLE_TWO_PI; \ - } - - -#define RC_UPDATE_FREQ 40. #define ROLL_COEF (STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ) // FIXME: unused, what was it supposed to be? //#define ROLL_COEF_H (STABILIZATION_ATTITUDE_SP_MAX_P_H / MAX_PPRZ) @@ -62,14 +50,42 @@ #define PITCH_DEADBAND_EXCEEDED() \ (radio_control.values[RADIO_PITCH] > STABILIZATION_ATTITUDE_DEADBAND_E || \ radio_control.values[RADIO_PITCH] < -STABILIZATION_ATTITUDE_DEADBAND_E) -#define YAW_DEADBAND_EXCEEDED() \ - (radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_DEADBAND_R || \ - radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R) -#define STABILIZATION_ATTITUDE_READ_RC(_sp, _in_flight) do { stabilization_attitude_read_rc_absolute(_sp, _in_flight); } while(0) -#define STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) do {} while(0) +#define STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) {} + +static inline void update_quat_from_eulers(struct Int32Quat *quat, struct Int32Eulers *eulers) { + struct Int32RMat rmat; + +#ifdef STICKS_RMAT312 + INT32_RMAT_OF_EULERS_312(rmat, *eulers); +#else + INT32_RMAT_OF_EULERS_321(rmat, *eulers); +#endif + INT32_QUAT_OF_RMAT(*quat, rmat); + INT32_QUAT_WRAP_SHORTEST(*quat); +} + + +static inline void stabilization_attitude_read_rc_setpoint(bool_t in_flight) { + + stab_att_sp_euler.phi = ((int32_t)-radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ); + stab_att_sp_euler.theta = ((int32_t) radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ); + + if (in_flight) { + if (YAW_DEADBAND_EXCEEDED()) { + stab_att_sp_euler.psi += ((int32_t)-radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); + INT32_ANGLE_NORMALIZE(stab_att_sp_euler.psi); + } + } + else { /* if not flying, use current yaw as setpoint */ + stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi; + } + + /* update quaternion setpoint from euler setpoint */ + update_quat_from_eulers(&stab_att_sp_quat, &stab_att_sp_euler); + +} void stabilization_attitude_ref_enter(void); -void stabilization_attitude_ref_schedule(uint8_t idx); #endif /* STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H */ diff --git a/sw/airborne/mcu_periph/sys_time.h b/sw/airborne/mcu_periph/sys_time.h index 6a6d59b442..40bf45d329 100644 --- a/sw/airborne/mcu_periph/sys_time.h +++ b/sw/airborne/mcu_periph/sys_time.h @@ -52,9 +52,9 @@ struct sys_time_timer { }; struct sys_time { - uint32_t nb_sec; ///< full seconds since startup - uint32_t nb_sec_rem; ///< remainder of second in CPU_TICKS - uint32_t nb_tick; ///< in SYS_TICKS with SYS_TIME_RESOLUTION + volatile uint32_t nb_sec; ///< full seconds since startup + volatile uint32_t nb_sec_rem; ///< remainder of second in CPU_TICKS + volatile uint32_t nb_tick; ///< in SYS_TICKS with SYS_TIME_RESOLUTION struct sys_time_timer timer[SYS_TIME_NB_TIMER]; }; diff --git a/sw/airborne/modules/digital_cam/led_cam_ctrl.h b/sw/airborne/modules/digital_cam/led_cam_ctrl.h index 6fe8ef72c8..541b691eed 100644 --- a/sw/airborne/modules/digital_cam/led_cam_ctrl.h +++ b/sw/airborne/modules/digital_cam/led_cam_ctrl.h @@ -102,6 +102,8 @@ static inline void dc_send_command(uint8_t cmd) DC_PUSH(DC_POWER_LED); break; #endif + default: + break; } } diff --git a/sw/airborne/subsystems/datalink/xbee.c b/sw/airborne/subsystems/datalink/xbee.c index 9d1c2aedf5..e2fe65e5c8 100644 --- a/sw/airborne/subsystems/datalink/xbee.c +++ b/sw/airborne/subsystems/datalink/xbee.c @@ -41,6 +41,8 @@ struct xbee_transport xbee_tp; #define AT_AP_MODE "ATAP1\r" #define AT_EXIT "ATCN\r" +uint32_t xbee_delay_time; + void xbee_init( void ) { xbee_tp.status = XBEE_UNINIT;