diff --git a/conf/autopilot/lisa_passthrough.makefile b/conf/autopilot/lisa_passthrough.makefile index ebf121e56c..d3a8188afd 100644 --- a/conf/autopilot/lisa_passthrough.makefile +++ b/conf/autopilot/lisa_passthrough.makefile @@ -10,6 +10,7 @@ SRC_BOOZ_ARCH=$(SRC_BOOZ)/arch/$(ARCH) SRC_LISA=lisa SRC_LISA_ARCH=$(SRC_LISA)/arch/$(ARCH) SRC_CSC=csc +SRC_BOARD=boards/$(BOARD) CFG_LISA_PASSTHROUGH = $(PAPARAZZI_SRC)/conf/autopilot/subsystems/lisa_passthrough @@ -17,7 +18,7 @@ CFG_LISA_PASSTHROUGH = $(PAPARAZZI_SRC)/conf/autopilot/subsystems/lisa_passthrou stm_passthrough.ARCHDIR = stm32 stm_passthrough.TARGET = stm_passthrough stm_passthrough.TARGETDIR = stm_passthrough -stm_passthrough.CFLAGS += -I$(SRC_LISA) -I$(SRC_LISA_ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) +stm_passthrough.CFLAGS += -I$(SRC_LISA) -I$(SRC_LISA_ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -I$(SRC_BOARD) stm_passthrough.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) stm_passthrough.CFLAGS += -DPERIPHERALS_AUTO_INIT stm_passthrough.srcs = $(SRC_LISA)/lisa_stm_passthrough_main.c \ @@ -85,7 +86,7 @@ stm_passthrough.srcs += $(SRC_BOOZ)/actuators/booz_actuators_pwm.c stm_passthrough.srcs += $(SRC_BOOZ_ARCH)/actuators/booz_actuators_pwm_arch.c # Baro -stm_passthrough.srcs += $(SRC_LISA)/lisa_baro.c +stm_passthrough.srcs += $(SRC_BOARD)/baro_board.c stm_passthrough.CFLAGS += -DUSE_I2C2 stm_passthrough.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c diff --git a/conf/autopilot/subsystems/lisa_passthrough/radio_control_spektrum.makefile b/conf/autopilot/subsystems/lisa_passthrough/radio_control_spektrum.makefile index e7029011b5..f0270e9f0d 100644 --- a/conf/autopilot/subsystems/lisa_passthrough/radio_control_spektrum.makefile +++ b/conf/autopilot/subsystems/lisa_passthrough/radio_control_spektrum.makefile @@ -1,5 +1,7 @@ +# +# Makefile for radio_control susbsytem in rotorcraft firmware +# -# Radio control ifndef RADIO_CONTROL_SPEKTRUM_MODEL RADIO_CONTROL_SPEKTRUM_MODEL=\"booz/radio_control/booz_radio_control_spektrum_dx7se.h\" @@ -8,10 +10,10 @@ endif stm_passthrough.CFLAGS += -DUSE_RADIO_CONTROL -DRADIO_CONTROL_BIND_IMPL_FUNC=radio_control_spektrum_try_bind stm_passthrough.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"booz/radio_control/booz_radio_control_spektrum.h\" stm_passthrough.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_MODEL_H=$(RADIO_CONTROL_SPEKTRUM_MODEL) -stm_passthrough.srcs += $(SRC_BOOZ)/booz_radio_control.c \ - $(SRC_BOOZ)/radio_control/booz_radio_control_spektrum.c -stm_passthrough.srcs += $(SRC_BOOZ_ARCH)/radio_control/booz_radio_control_spektrum_arch.c -stm_passthrough.CFLAGS += -DRADIO_CONTROL_LED=5 -stm_passthrough.CFLAGS += -DUSE_UART3 -DUART3_BAUD=B115200 -stm_passthrough.CFLAGS += -DRADIO_CONTROL_LINK=Uart3 +stm_passthrough.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) +stm_passthrough.CFLAGS += -DRADIO_CONTROL_LINK=$(RADIO_CONTROL_LINK) +stm_passthrough.CFLAGS += -DUSE_$(RADIO_CONTROL_LINK) -D$(RADIO_CONTROL_LINK)_BAUD=B115200 +stm_passthrough.srcs += $(SRC_BOOZ)/booz_radio_control.c \ + $(SRC_BOOZ)/radio_control/booz_radio_control_spektrum.c \ + $(SRC_BOOZ_ARCH)/radio_control/booz_radio_control_spektrum_arch.c diff --git a/sw/airborne/lisa/lisa_stm_passthrough_main.c b/sw/airborne/lisa/lisa_stm_passthrough_main.c index f70d30e3e5..923c1a4967 100644 --- a/sw/airborne/lisa/lisa_stm_passthrough_main.c +++ b/sw/airborne/lisa/lisa_stm_passthrough_main.c @@ -36,7 +36,7 @@ #include "csc_msg_def.h" #include "csc_protocol.h" -#include "lisa/lisa_baro.h" +#include "rotorcraft/baro.h" static inline void main_init(void); @@ -62,102 +62,103 @@ struct CscVaneMsg csc_vane_msg; int main(void) { - main_init(); + main_init(); - while (1) { - if (sys_time_periodic()) - main_periodic(); - main_event(); - } - - return 0; + while (1) { + if (sys_time_periodic()) + main_periodic(); + main_event(); + } + + return 0; } static inline void main_init(void) { - hw_init(); - sys_time_init(); - booz_imu_init(); - baro_init(); - radio_control_init(); - booz_actuators_init(); - overo_link_init(); - cscp_init(); -// csc_bat_monitor_init(); + hw_init(); + sys_time_init(); + booz_imu_init(); + baro_init(); + radio_control_init(); + booz_actuators_init(); + overo_link_init(); + cscp_init(); + // csc_bat_monitor_init(); + + cscp_register_callback(CSC_VANE_MSG_ID, on_vane_msg, (void *)&csc_vane_msg); + new_radio_msg = FALSE; - cscp_register_callback(CSC_VANE_MSG_ID, on_vane_msg, (void *)&csc_vane_msg); - new_radio_msg = FALSE; } static inline void main_periodic(void) { - booz_imu_periodic(); - OveroLinkPeriodic(on_overo_link_lost); -// csc_bat_monitor_periodic(); + booz_imu_periodic(); + OveroLinkPeriodic(on_overo_link_lost); + // csc_bat_monitor_periodic(); - RunOnceEvery(10, { - LED_PERIODIC(); - DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM); - radio_control_periodic(); - }); + RunOnceEvery(10, { + LED_PERIODIC(); + DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM); + radio_control_periodic(); + }); RunOnceEvery(2, {baro_periodic();}); } static inline void on_rc_message(void) { - new_radio_msg = TRUE; + new_radio_msg = TRUE; } static inline void on_overo_link_msg_received(void) { - /* IMU up */ - overo_link.up.msg.valid.imu = 1; - RATES_COPY(overo_link.up.msg.gyro, booz_imu.gyro); - VECT3_COPY(overo_link.up.msg.accel, booz_imu.accel); - VECT3_COPY(overo_link.up.msg.mag, booz_imu.mag); - - /* RC up */ - overo_link.up.msg.valid.rc = new_radio_msg; - new_radio_msg = FALSE; - - overo_link.up.msg.rc_pitch = radio_control.values[RADIO_CONTROL_PITCH]; - overo_link.up.msg.rc_roll = radio_control.values[RADIO_CONTROL_ROLL]; - overo_link.up.msg.rc_yaw = radio_control.values[RADIO_CONTROL_YAW]; - overo_link.up.msg.rc_thrust = radio_control.values[RADIO_CONTROL_THROTTLE]; - overo_link.up.msg.rc_mode = radio_control.values[RADIO_CONTROL_MODE]; + /* IMU up */ + overo_link.up.msg.valid.imu = 1; + RATES_COPY(overo_link.up.msg.gyro, booz_imu.gyro); + VECT3_COPY(overo_link.up.msg.accel, booz_imu.accel); + VECT3_COPY(overo_link.up.msg.mag, booz_imu.mag); + + /* RC up */ + overo_link.up.msg.valid.rc = new_radio_msg; + new_radio_msg = FALSE; + + overo_link.up.msg.rc_pitch = radio_control.values[RADIO_CONTROL_PITCH]; + overo_link.up.msg.rc_roll = radio_control.values[RADIO_CONTROL_ROLL]; + overo_link.up.msg.rc_yaw = radio_control.values[RADIO_CONTROL_YAW]; + overo_link.up.msg.rc_thrust = radio_control.values[RADIO_CONTROL_THROTTLE]; + overo_link.up.msg.rc_mode = radio_control.values[RADIO_CONTROL_MODE]; #ifdef RADIO_CONTROL_KILL - overo_link.up.msg.rc_kill = radio_control.values[RADIO_CONTROL_KILL]; + overo_link.up.msg.rc_kill = radio_control.values[RADIO_CONTROL_KILL]; #endif #ifdef RADIO_CONTROL_GEAR - overo_link.up.msg.rc_gear = radio_control.values[RADIO_CONTROL_GEAR]; + overo_link.up.msg.rc_gear = radio_control.values[RADIO_CONTROL_GEAR]; #endif - overo_link.up.msg.rc_aux3 = radio_control.values[RADIO_CONTROL_AUX3]; - overo_link.up.msg.rc_aux4 = radio_control.values[RADIO_CONTROL_AUX4]; - overo_link.up.msg.rc_status = radio_control.status; - - overo_link.up.msg.stm_msg_cnt = overo_link.msg_cnt; - overo_link.up.msg.stm_crc_err_cnt = overo_link.crc_err_cnt; - - /* baro up */ - overo_link.up.msg.valid.pressure_differential = new_baro_diff; - overo_link.up.msg.valid.pressure_absolute = new_baro_abs; - new_baro_diff = FALSE; - new_baro_abs = FALSE; - overo_link.up.msg.pressure_differential = baro.diff_raw; - overo_link.up.msg.pressure_absolute = baro.abs_raw; - - overo_link.up.msg.valid.vane = new_vane; - new_vane = FALSE; - overo_link.up.msg.vane_angle1 = csc_vane_msg.vane_angle1; - overo_link.up.msg.vane_angle2 = csc_vane_msg.vane_angle2; - - /* pwm acuators down */ - for (int i = 0; i < LISA_PWM_OUTPUT_NB; i++) { - booz_actuators_pwm_values[i] = overo_link.down.msg.pwm_outputs_usecs[i]; - } - booz_actuators_pwm_commit(); - + overo_link.up.msg.rc_aux3 = radio_control.values[RADIO_CONTROL_AUX3]; + overo_link.up.msg.rc_aux4 = radio_control.values[RADIO_CONTROL_AUX4]; + overo_link.up.msg.rc_status = radio_control.status; + + overo_link.up.msg.stm_msg_cnt = overo_link.msg_cnt; + overo_link.up.msg.stm_crc_err_cnt = overo_link.crc_err_cnt; + + /* baro up */ + overo_link.up.msg.valid.pressure_differential = new_baro_diff; + overo_link.up.msg.valid.pressure_absolute = new_baro_abs; + new_baro_diff = FALSE; + new_baro_abs = FALSE; + overo_link.up.msg.pressure_differential = baro.differential; + overo_link.up.msg.pressure_absolute = baro.absolute; + + overo_link.up.msg.valid.vane = new_vane; + new_vane = FALSE; + overo_link.up.msg.vane_angle1 = csc_vane_msg.vane_angle1; + overo_link.up.msg.vane_angle2 = csc_vane_msg.vane_angle2; + + /* pwm acuators down */ + for (int i = 0; i < LISA_PWM_OUTPUT_NB; i++) { + booz_actuators_pwm_values[i] = overo_link.down.msg.pwm_outputs_usecs[i]; + } + booz_actuators_pwm_commit(); + } static inline void on_overo_link_lost(void) { @@ -167,33 +168,33 @@ static inline void on_overo_link_crc_failed(void) { } static inline void on_gyro_accel_event(void) { - BoozImuScaleGyro(booz_imu); - BoozImuScaleAccel(booz_imu); + BoozImuScaleGyro(booz_imu); + BoozImuScaleAccel(booz_imu); } static inline void on_mag_event(void) { - BoozImuScaleMag(booz_imu); + BoozImuScaleMag(booz_imu); } static inline void on_vane_msg(void *data) { - new_vane = TRUE; - int zero = 0; - DOWNLINK_SEND_VANE_SENSOR(DefaultChannel, - &(csc_vane_msg.vane_angle1), - &zero, - &zero, - &zero, - &zero, - &csc_vane_msg.vane_angle2, - &zero, - &zero, - &zero, - &zero); + new_vane = TRUE; + int zero = 0; + DOWNLINK_SEND_VANE_SENSOR(DefaultChannel, + &(csc_vane_msg.vane_angle1), + &zero, + &zero, + &zero, + &zero, + &csc_vane_msg.vane_angle2, + &zero, + &zero, + &zero, + &zero); } static inline void main_on_baro_diff(void) { new_baro_diff = TRUE; - RunOnceEvery(5,{DOWNLINK_SEND_BOOZ_BARO2_RAW(DefaultChannel, &baro.abs_raw, &baro.diff_raw);}); + RunOnceEvery(5,{DOWNLINK_SEND_BARO_RAW(DefaultChannel, &baro.absolute, &baro.differential);}); } static inline void main_on_baro_abs(void) { @@ -202,11 +203,12 @@ static inline void main_on_baro_abs(void) { static inline void main_event(void) { - BoozImuEvent(on_gyro_accel_event, on_mag_event); - BaroEvent(main_on_baro_abs, main_on_baro_diff); - OveroLinkEvent(on_overo_link_msg_received, on_overo_link_crc_failed); - RadioControlEvent(on_rc_message); - cscp_event(); + BoozImuEvent(on_gyro_accel_event, on_mag_event); + BaroEvent(main_on_baro_abs, main_on_baro_diff); + OveroLinkEvent(on_overo_link_msg_received, on_overo_link_crc_failed); + RadioControlEvent(on_rc_message); + cscp_event(); + }