diff --git a/conf/airframes/airframe.dtd b/conf/airframes/airframe.dtd index c9ff8cb16f..5026d8435f 100644 --- a/conf/airframes/airframe.dtd +++ b/conf/airframes/airframe.dtd @@ -1,8 +1,12 @@ - + + + + + @@ -27,6 +31,7 @@ name CDATA #IMPLIED> + + + + + + + diff --git a/conf/airframes/mercury.xml b/conf/airframes/mercury.xml index 6dd8d3f284..23bc06e4ab 100644 --- a/conf/airframes/mercury.xml +++ b/conf/airframes/mercury.xml @@ -31,10 +31,10 @@
- - - - + + + + @@ -74,9 +74,10 @@ - - - + + + + @@ -95,9 +96,10 @@ - - - + + + + @@ -111,9 +113,10 @@ - - - + + + + @@ -132,6 +135,18 @@
+
+ + + + + + + + + +
+
@@ -172,6 +187,7 @@ include $(PAPARAZZI_SRC)/conf/autopilot/mercury.makefile include $(PAPARAZZI_SRC)/conf/autopilot/csc_baro.makefile +include $(PAPARAZZI_SRC)/conf/autopilot/subsystems/booz2_radio_control_dummy.makefile diff --git a/conf/autopilot/mercury.makefile b/conf/autopilot/mercury.makefile index d10d51ed2f..d7d746d683 100644 --- a/conf/autopilot/mercury.makefile +++ b/conf/autopilot/mercury.makefile @@ -74,12 +74,12 @@ ap.srcs += $(SRC_CSC)/csc_datalink.c ap.CFLAGS += -DPPRZ_TRIG_CONST=const ap.srcs += $(SRC_CSC)/mercury_xsens.c $(SRC_BOOZ)/booz_imu.c math/pprz_trig_int.c -ap.CFLAGS += -DXSENS1_LINK=Uart0 -DBOOZ2_IMU_TYPE=\"mercury_xsens.h\" +ap.CFLAGS += -DXSENS1_LINK=Uart0 -DBOOZ_IMU_TYPE_H=\"mercury_xsens.h\" -ap.srcs += $(SRC_BOOZ)/booz2_filter_attitude_cmpl_euler.c $(SRC_BOOZ)/booz_ahrs_aligner.c +ap.srcs += $(SRC_BOOZ)/ahrs/booz2_filter_attitude_cmpl_euler.c $(SRC_BOOZ)/ahrs/booz_ahrs_aligner.c ap.CFLAGS += -DFLOAT_T=float -ap.srcs += $(SRC_BOOZ)/booz2_stabilization.c $(SRC_BOOZ)/booz2_stabilization_attitude.c +ap.srcs += $(SRC_BOOZ)/booz_stabilization.c $(SRC_BOOZ)/stabilization/booz_stabilization_attitude.c $(SRC_BOOZ)/stabilization/booz_stabilization_rate.c # AHI copied from booz w/ light modifications for vertical control ap.CFLAGS += -DUSE_VFF -DDT_VFILTER="(1./512.)" -DFLOAT_T=float diff --git a/conf/settings/mercury.xml b/conf/settings/mercury.xml index e636c9b689..ce51961b5c 100644 --- a/conf/settings/mercury.xml +++ b/conf/settings/mercury.xml @@ -5,8 +5,8 @@ - + @@ -15,22 +15,22 @@ - - - - - - - - - - - - + + + + + + + + + + + + - + @@ -39,7 +39,14 @@ - + + + + + + + + diff --git a/sw/airborne/csc/csc_autopilot.h b/sw/airborne/csc/csc_autopilot.h index a1948ea8c2..a6483ed68b 100644 --- a/sw/airborne/csc/csc_autopilot.h +++ b/sw/airborne/csc/csc_autopilot.h @@ -27,7 +27,7 @@ #include "types.h" #include "std.h" -#include "pprz_algebra_float.h" +#include "math/pprz_algebra_float.h" struct control_gains { float pitch_kp; diff --git a/sw/airborne/csc/csc_telemetry.h b/sw/airborne/csc/csc_telemetry.h index 7008b29b92..4289bceb54 100644 --- a/sw/airborne/csc/csc_telemetry.h +++ b/sw/airborne/csc/csc_telemetry.h @@ -63,12 +63,11 @@ #define PERIODIC_SEND_ACTUATORS() DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators) #endif -#ifdef RADIO_CONTROL -#include "radio_control.h" -#define PERIODIC_SEND_PPM() DOWNLINK_SEND_PPM(&last_ppm_cpt, PPM_NB_PULSES, ppm_pulses) -#define PERIODIC_SEND_RC() DOWNLINK_SEND_RC(PPM_NB_PULSES, rc_values) -#define PERIODIC_SEND_QUAD_STATUS() DOWNLINK_SEND_QUAD_STATUS(&rc_status, &pprz_mode, &vsupply, &cpu_time) -#endif /* RADIO_CONTROL */ +#ifdef USE_RADIO_CONTROL +#include "booz_radio_control.h" +#define PERIODIC_SEND_RC() DOWNLINK_SEND_RC(RADIO_CONTROL_NB_CHANNEL, radio_control.values) +#define PERIODIC_SEND_QUAD_STATUS() DOWNLINK_SEND_QUAD_STATUS(&radio_control.status, &pprz_mode, &vsupply, &cpu_time) +#endif /* USE_RADIO_CONTROL */ extern uint8_t telemetry_mode_Ap; diff --git a/sw/airborne/csc/csc_xsens.c b/sw/airborne/csc/csc_xsens.c index c8d589f7cf..0fcf28637a 100644 --- a/sw/airborne/csc/csc_xsens.c +++ b/sw/airborne/csc/csc_xsens.c @@ -36,7 +36,7 @@ #include "messages.h" #include "uart.h" //#include "com_stats.h" -#include "pprz_algebra_float.h" +#include "math/pprz_algebra_float.h" #include "string.h" void parse_xsens_msg(uint8_t xsens_id, uint8_t c ); @@ -211,6 +211,7 @@ static uint8_t xsens_msg_idx[XSENS_COUNT]; static uint8_t ck[XSENS_COUNT]; static uint8_t send_ck[XSENS_COUNT]; + void xsens_init( void ) { for (int i = 0; i < XSENS_COUNT; i++) { diff --git a/sw/airborne/csc/csc_xsens.h b/sw/airborne/csc/csc_xsens.h index ae9740e11b..0d031110f7 100644 --- a/sw/airborne/csc/csc_xsens.h +++ b/sw/airborne/csc/csc_xsens.h @@ -26,7 +26,7 @@ #define __CSC_XSENS_H__ #include "types.h" -#include "pprz_algebra_float.h" +#include "math/pprz_algebra_float.h" #define XSENS_COUNT 1 diff --git a/sw/airborne/csc/mercury_ap.c b/sw/airborne/csc/mercury_ap.c index ca9161f65e..caf4839bc5 100644 --- a/sw/airborne/csc/mercury_ap.c +++ b/sw/airborne/csc/mercury_ap.c @@ -27,12 +27,12 @@ #include "commands.h" #include "mercury_xsens.h" #include "booz2_autopilot.h" -#include "booz2_stabilization.h" -#include "booz2_stabilization_attitude.h" +#include "booz_stabilization.h" +#include "stabilization/booz_stabilization_attitude.h" #include "led.h" -#include "pprz_algebra_float.h" +#include "math/pprz_algebra_float.h" #include "string.h" -#include "radio_control.h" +#include "booz_radio_control.h" #include "mercury_supervision.h" #include "actuators.h" #include "props_csc.h" @@ -54,6 +54,9 @@ uint32_t booz2_autopilot_in_flight_counter; uint16_t mercury_yaw_servo_gain; +uint8_t props_enable[PROPS_NB]; +uint8_t props_throttle_pass; + #define BOOZ2_AUTOPILOT_MOTOR_ON_TIME (512/4) #define BOOZ2_AUTOPILOT_IN_FLIGHT_TIME 40 @@ -69,6 +72,11 @@ void csc_ap_init(void) { booz2_autopilot_in_flight_counter = 0; booz2_autopilot_mode_auto2 = BOOZ2_MODE_AUTO2; booz2_autopilot_tol = 0; + + props_throttle_pass = 0; + for(uint8_t i = 0; i < PROPS_NB; i++){ + props_enable[i] = 1; + } } @@ -76,7 +84,7 @@ void csc_ap_init(void) { #define BOOZ2_AUTOPILOT_CHECK_IN_FLIGHT() { \ if (booz2_autopilot_in_flight) { \ if (booz2_autopilot_in_flight_counter > 0) { \ - if (rc_values[RADIO_THROTTLE] < BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD) { \ + if (radio_control.values[RADIO_CONTROL_THROTTLE] < BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD) { \ booz2_autopilot_in_flight_counter--; \ if (booz2_autopilot_in_flight_counter == 0) { \ booz2_autopilot_in_flight = FALSE; \ @@ -90,7 +98,7 @@ void csc_ap_init(void) { else { /* not in flight */ \ if (booz2_autopilot_in_flight_counter < BOOZ2_AUTOPILOT_IN_FLIGHT_TIME && \ booz2_autopilot_motors_on) { \ - if (rc_values[RADIO_THROTTLE] > BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD) { \ + if (radio_control.values[RADIO_CONTROL_THROTTLE] > BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD) { \ booz2_autopilot_in_flight_counter++; \ if (booz2_autopilot_in_flight_counter == BOOZ2_AUTOPILOT_IN_FLIGHT_TIME) \ booz2_autopilot_in_flight = TRUE; \ @@ -104,9 +112,9 @@ void csc_ap_init(void) { #define BOOZ2_AUTOPILOT_CHECK_MOTORS_ON() { \ if(!booz2_autopilot_motors_on){ \ - if (rc_values[RADIO_THROTTLE] < BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD && \ - (rc_values[RADIO_YAW] > BOOZ2_AUTOPILOT_YAW_TRESHOLD || \ - rc_values[RADIO_YAW] < -BOOZ2_AUTOPILOT_YAW_TRESHOLD)) { \ + if (radio_control.values[RADIO_CONTROL_THROTTLE] < BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD && \ + (radio_control.values[RADIO_CONTROL_YAW] > BOOZ2_AUTOPILOT_YAW_TRESHOLD || \ + radio_control.values[RADIO_CONTROL_YAW] < -BOOZ2_AUTOPILOT_YAW_TRESHOLD)) { \ if ( booz2_autopilot_motors_on_counter < BOOZ2_AUTOPILOT_MOTOR_ON_TIME) { \ booz2_autopilot_motors_on_counter++; \ if (booz2_autopilot_motors_on_counter == BOOZ2_AUTOPILOT_MOTOR_ON_TIME){ \ @@ -142,27 +150,39 @@ void csc_ap_periodic(uint8_t _in_flight, uint8_t kill) { if(kill) booz2_autopilot_motors_on = FALSE; booz2_autopilot_in_flight = _in_flight; - booz2_stabilization_attitude_read_rc(booz2_autopilot_in_flight); - booz2_stabilization_attitude_run(booz2_autopilot_in_flight); + booz_stabilization_attitude_read_rc(booz2_autopilot_in_flight); + booz_stabilization_attitude_run(booz2_autopilot_in_flight); booz2_guidance_v_run(booz2_autopilot_in_flight); - booz2_stabilization_cmd[COMMAND_THRUST] = (int32_t)rc_values[RADIO_THROTTLE] * 105 / 7200 + 95; + booz_stabilization_cmd[COMMAND_THRUST] = (int32_t)radio_control.values[RADIO_CONTROL_THROTTLE] * 105 / 7200 + 95; - CscSetCommands(booz2_stabilization_cmd, + CscSetCommands(booz_stabilization_cmd, booz2_autopilot_in_flight,booz2_autopilot_motors_on); BOOZ2_SUPERVISION_RUN(mixed_commands, commands, booz2_autopilot_motors_on); - props_set(PROP_UPPER_LEFT, mixed_commands[PROP_UPPER_LEFT]); - props_set(PROP_LOWER_RIGHT,mixed_commands[PROP_LOWER_RIGHT]); - props_set(PROP_LOWER_LEFT, mixed_commands[PROP_LOWER_LEFT]); - props_set(PROP_UPPER_RIGHT,mixed_commands[PROP_UPPER_RIGHT]); + + + if(booz2_autopilot_motors_on && props_throttle_pass){ + Bound(booz2_stabilization_cmd[COMMAND_THRUST],0,255); + for(uint8_t i = 0; i < PROPS_NB; i++) + mixed_commands[i] = booz2_stabilization_cmd[COMMAND_THRUST]; + + } + + for(uint8_t i = 0; i < PROPS_NB; i++){ + if(props_enable[i]) + props_set(i,mixed_commands[i]); + else + props_set(i,0); + } + props_commit(); MERCURY_SURFACES_SUPERVISION_RUN(Actuator, - booz2_stabilization_cmd, + booz_stabilization_cmd, mixed_commands, (!booz2_autopilot_in_flight)); ActuatorsCommit(); diff --git a/sw/airborne/csc/mercury_ap.h b/sw/airborne/csc/mercury_ap.h index 2924ead6d4..da89b3f3d7 100644 --- a/sw/airborne/csc/mercury_ap.h +++ b/sw/airborne/csc/mercury_ap.h @@ -28,6 +28,8 @@ #include "airframe.h" extern pprz_t mixed_commands[PROPS_NB]; +extern uint8_t props_enable[PROPS_NB]; +extern uint8_t props_throttle_pass; void csc_ap_init( void ); void csc_ap_periodic (uint8_t _in_flight, uint8_t kill); diff --git a/sw/airborne/csc/mercury_main.c b/sw/airborne/csc/mercury_main.c index 5fe581c17f..a586a6df91 100644 --- a/sw/airborne/csc/mercury_main.c +++ b/sw/airborne/csc/mercury_main.c @@ -40,19 +40,20 @@ #include "csc_msg_def.h" #include ACTUATORS -#include "booz2_imu.h" -#include "booz_ahrs_aligner.h" -#include "booz_ahrs.h" +#include "booz/booz_imu.h" +#include "booz/ahrs/booz_ahrs_aligner.h" +#include "booz/booz_ahrs.h" #include "mercury_xsens.h" #include "csc_telemetry.h" #include "csc_adc.h" #include "mercury_ap.h" -#include "booz2_autopilot.h" -#include "booz2_guidance_v.h" +#include "booz/booz2_autopilot.h" +#include "booz/guidance/booz2_guidance_v.h" #include "csc_can.h" #include "csc_baro.h" +#include "booz_radio_control.h" -#include "booz2_stabilization_attitude.h" +#include "booz/stabilization/booz_stabilization_attitude.h" extern uint8_t vsupply; @@ -89,17 +90,17 @@ static void on_rc_cmd(struct CscRCMsg *msg) { uint16_t aux2_flag; - 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); + radio_control.values[RADIO_CONTROL_ROLL] = CSC_RC_SCALE*(msg->right_stick_horizontal - CSC_RC_OFFSET); + radio_control.values[RADIO_CONTROL_PITCH] = -CSC_RC_SCALE*(msg->right_stick_vertical - CSC_RC_OFFSET); + radio_control.values[RADIO_CONTROL_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; - rc_values[RADIO_MODE2] = (aux2_flag == 0) ? -7000 : ( (aux2_flag == 1) ? 0 : 7000); - rc_values[RADIO_MODE] = (pprz_mode == 0) ? -7000 : ( (pprz_mode == 1) ? 0 : 7000); - rc_values[RADIO_THROTTLE] = -CSC_RC_SCALE*((msg->left_stick_vertical_and_flap_mix & ~(3 << 13)) - CSC_RC_OFFSET); + radio_control.values[RADIO_CONTROL_MODE2] = (aux2_flag == 0) ? -7000 : ( (aux2_flag == 1) ? 0 : 7000); + radio_control.values[RADIO_CONTROL_MODE] = (pprz_mode == 0) ? -7000 : ( (pprz_mode == 1) ? 0 : 7000); + radio_control.values[RADIO_CONTROL_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; + radio_control.time_since_last_frame = 0; + radio_control.status = RADIO_CONTROL_OK; } static inline void csc_main_init( void ) { @@ -111,19 +112,18 @@ static inline void csc_main_init( void ) { Uart0Init(); Uart1Init(); - booz2_imu_init(); + booz_imu_init(); booz_ahrs_aligner_init(); booz_ahrs_init(); xsens_init(); - booz2_stabilization_attitude_init(); + booz_stabilization_attitude_init(); booz2_guidance_v_init(); booz_ins_init(); csc_adc_init(); - ppm_init(); baro_scp_init(); @@ -138,7 +138,7 @@ static inline void csc_main_init( void ) { csc_ap_init(); int_enable(); - booz2_stabilization_attitude_enter(); + booz_stabilization_attitude_enter(); } @@ -147,7 +147,7 @@ static inline void csc_main_periodic( void ) static uint32_t csc_loops = 0; PeriodicSendAp(); - radio_control_periodic_task(); + radio_control_periodic(); cpu_time++; diff --git a/sw/airborne/csc/mercury_supervision.h b/sw/airborne/csc/mercury_supervision.h index 68aedce137..b90aa96680 100644 --- a/sw/airborne/csc/mercury_supervision.h +++ b/sw/airborne/csc/mercury_supervision.h @@ -72,8 +72,8 @@ extern pprz_t mercury_supervision_yaw_comp_offset; #define MERCURY_SURFACES_SUPERVISION_RUN(_actuators,_cmds,_props,_surfaces_manual) { \ if (_surfaces_manual) { \ - _actuators(SERVO_S1) = (SERVO_S1_MAX+SERVO_S1_MIN)/2 +((SERVO_S1_MAX-SERVO_S1_MIN)*rc_values[RADIO_YAW])/2/7200; \ - _actuators(SERVO_S2) = (SERVO_S2_MAX+SERVO_S2_MIN)/2 +((SERVO_S2_MAX-SERVO_S2_MIN)*rc_values[RADIO_YAW])/2/7200; \ + _actuators(SERVO_S1) = (SERVO_S1_MAX+SERVO_S1_MIN)/2 +((SERVO_S1_MAX-SERVO_S1_MIN)*radio_control.values[RADIO_CONTROL_YAW])/2/7200; \ + _actuators(SERVO_S2) = (SERVO_S2_MAX+SERVO_S2_MIN)/2 +((SERVO_S2_MAX-SERVO_S2_MIN)*radio_control.values[RADIO_CONTROL_YAW])/2/7200; \ } else { \ int32_t bndcmd = (mercury_supervision_yaw_servo_gain*_cmds[COMMAND_YAW]*mercury_supervision_yaw_comp_offset)/(mercury_supervision_yaw_comp_slope*((_props[PROP_UPPER_RIGHT]+_props[PROP_UPPER_LEFT])/2 - 105) + mercury_supervision_yaw_comp_offset); \ Bound(bndcmd,-400,400); \ diff --git a/sw/airborne/csc/mercury_xsens.c b/sw/airborne/csc/mercury_xsens.c index 38645b7b3d..f156d1d7b7 100644 --- a/sw/airborne/csc/mercury_xsens.c +++ b/sw/airborne/csc/mercury_xsens.c @@ -27,9 +27,10 @@ */ #include "mercury_xsens.h" -#include "booz2_imu.h" -#include "booz_ahrs.h" -#include "booz_ahrs_aligner.h" +#include "booz/booz_imu.h" +#include "booz/booz_ahrs.h" +#include "booz/ahrs/booz_ahrs_aligner.h" +#include "booz/booz_imu.h" #include "csc_booz2_ins.h" #include @@ -40,7 +41,7 @@ #include "messages.h" #include "uart.h" //#include "com_stats.h" -#include "pprz_algebra_float.h" +#include "math/pprz_algebra_float.h" #include "string.h" void parse_xsens_msg(uint8_t xsens_id, uint8_t c ); @@ -201,7 +202,11 @@ static uint8_t xsens_msg_idx[XSENS_COUNT]; static uint8_t ck[XSENS_COUNT]; static uint8_t send_ck[XSENS_COUNT]; - +void booz_imu_impl_init( void ) +{ + +} + void xsens_init( void ) { for (int i = 0; i < XSENS_COUNT; i++) { @@ -305,9 +310,9 @@ void xsens_parse_msg( uint8_t xsens_id ) { booz_imu.mag_unscaled.x = XSENS_DATA_RAWInertial_magZ(xsens_msg_buf[xsens_id][buf_slot],offset); booz_imu.mag_unscaled.y = XSENS_DATA_RAWInertial_magY(xsens_msg_buf[xsens_id][buf_slot],offset); booz_imu.mag_unscaled.z = XSENS_DATA_RAWInertial_magX(xsens_msg_buf[xsens_id][buf_slot],offset); - Booz2ImuScaleGyro(); - Booz2ImuScaleAccel(); - Booz2ImuScaleMag(); + BoozImuScaleGyro(); + BoozImuScaleAccel(); + BoozImuScaleMag(); // Copied from booz2_main -- 5143134f060fcc57ce657e17d8b7fc2e72119fd7 // mmt 6/15/09