diff --git a/conf/airframes/csc_ap.xml b/conf/airframes/csc_ap.xml
index 385c11afc9..72ab3b9be3 100644
--- a/conf/airframes/csc_ap.xml
+++ b/conf/airframes/csc_ap.xml
@@ -26,6 +26,7 @@
+include $(PAPARAZZI_SRC)/conf/autopilot/pwm_input.makefile
include $(PAPARAZZI_SRC)/conf/autopilot/csc_ap.makefile
diff --git a/conf/airframes/mercury.xml b/conf/airframes/mercury.xml
index b9e846f2c5..29e64c23ab 100644
--- a/conf/airframes/mercury.xml
+++ b/conf/airframes/mercury.xml
@@ -8,8 +8,8 @@
-->
-
-
+
+
+
+
@@ -113,12 +120,12 @@
-
+
-
-
+
+
@@ -133,8 +140,8 @@
-
-
+
+
diff --git a/conf/airframes/mercury_csc.xml b/conf/airframes/mercury_csc.xml
index 23aaba3767..73a9898b54 100644
--- a/conf/airframes/mercury_csc.xml
+++ b/conf/airframes/mercury_csc.xml
@@ -28,8 +28,13 @@ CSC_ID=0
SRC_BOOZ=booz
SRC_BOOZ_ARCH=booz/arm7
ap.CFLAGS += -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH)
+
+ap.CFLAGS += -DSPEKTRUM_LINK=Uart1 -DUSE_UART1 -DUART1_BAUD=B115200 -DUART1_VIC_SLOT=6
+ap.srcs += $(SRC_CSC)/csc_rc_spektrum.c
+
include $(PAPARAZZI_SRC)/conf/autopilot/mercury_csc.makefile
+
diff --git a/conf/autopilot/mercury.makefile b/conf/autopilot/mercury.makefile
index 2b5d988c97..e48de623cb 100644
--- a/conf/autopilot/mercury.makefile
+++ b/conf/autopilot/mercury.makefile
@@ -87,7 +87,7 @@ ap.srcs += $(SRC_CSC)/csc_booz2_ins.c $(SRC_CSC)/csc_booz2_hf_float.c $(SRC_CSC)
ap.srcs += $(SRC_CSC)/csc_booz2_guidance_v.c
-ap.CFLAGS += -DAP_LINK_CAN -DCAN_LED=2
+ap.CFLAGS += -DAP_LINK_CAN # -DCAN_LED=2
ap.CFLAGS += -DUSE_CAN1 -DCAN1_BTR=CANBitrate125k_3MHz
ap.CFLAGS += -DCAN1_VIC_SLOT=3 -DCAN1_ERR_VIC_SLOT=7
ap.srcs += $(SRC_CSC)/csc_can.c
@@ -109,6 +109,8 @@ ap.CFLAGS += -DRADIO_CONTROL
ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
ap.srcs += $(SRC_CSC)/mercury_ap.c
+ap.srcs += $(SRC_CSC)/mercury_supervision.c
+
ap.CFLAGS += -DERROR_LED=4
diff --git a/conf/autopilot/mercury_csc.makefile b/conf/autopilot/mercury_csc.makefile
index bfa737ec8d..7c6155644c 100644
--- a/conf/autopilot/mercury_csc.makefile
+++ b/conf/autopilot/mercury_csc.makefile
@@ -95,15 +95,8 @@ ap.srcs += $(SRC_CSC_ARCH)/buss_twi_blmc_hw.c
ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-
ap.srcs += $(SRC_CSC)/csc_adc.c
-ap.CFLAGS += -DTHROTTLE_LINK=Uart0 -DTHROTTLE_LED=3
-ap.srcs += $(SRC_CSC)/csc_throttle.c
-
-ap.CFLAGS += -DSPEKTRUM_LINK=Uart1 -DUSE_UART1 -DUART1_BAUD=B115200 -DUART1_VIC_SLOT=6
-ap.srcs += $(SRC_CSC)/csc_rc_spektrum.c
-
ap.CFLAGS += -DERROR_LED=4
#
diff --git a/conf/settings/csc_ap.xml b/conf/settings/csc_ap.xml
index 5d46b36c3a..630175465a 100644
--- a/conf/settings/csc_ap.xml
+++ b/conf/settings/csc_ap.xml
@@ -11,18 +11,26 @@
+
+
+
+
+
-
+
-
+
-
-
+
+
+
+
+
diff --git a/conf/settings/mercury.xml b/conf/settings/mercury.xml
index 4e202a6c28..e636c9b689 100644
--- a/conf/settings/mercury.xml
+++ b/conf/settings/mercury.xml
@@ -33,6 +33,13 @@
+
+
+
+
+
+
+
diff --git a/conf/telemetry/csc_ap.xml b/conf/telemetry/csc_ap.xml
index 5834ed3a3d..85d7651571 100644
--- a/conf/telemetry/csc_ap.xml
+++ b/conf/telemetry/csc_ap.xml
@@ -4,8 +4,10 @@
-
-
+
+
+
+
diff --git a/conf/telemetry/mercury.xml b/conf/telemetry/mercury.xml
index 4b1a25cf31..d0bb728ad0 100644
--- a/conf/telemetry/mercury.xml
+++ b/conf/telemetry/mercury.xml
@@ -5,15 +5,21 @@
-
-
-
+
+
+
+
+
+
+
+
+
-
+
diff --git a/sw/airborne/csc/csc_ap_main.c b/sw/airborne/csc/csc_ap_main.c
index c64f135c09..82bde5ac7c 100644
--- a/sw/airborne/csc/csc_ap_main.c
+++ b/sw/airborne/csc/csc_ap_main.c
@@ -44,6 +44,7 @@
#include "csc_xsens.h"
#include "csc_autopilot.h"
#include "csc_can.h"
+#include "pwm_input.h"
#define CSC_STATUS_TIMEOUT (SYS_TICS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD)
@@ -53,6 +54,7 @@
#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;
@@ -71,11 +73,16 @@ static void nop(struct CscCanMsg *msg)
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 - 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;
@@ -101,6 +108,10 @@ static void csc_main_init( void ) {
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);
@@ -130,7 +141,7 @@ static void csc_main_periodic( void )
}
xsens_periodic_task();
if (pprz_mode == PPRZ_MODE_AUTO1)
- csc_ap_periodic();
+ csc_ap_periodic(cpu_time);
if (csc_trims_set) {
csc_trims_set = 0;
diff --git a/sw/airborne/csc/csc_autopilot.c b/sw/airborne/csc/csc_autopilot.c
index d8906c6d11..45725219c6 100644
--- a/sw/airborne/csc/csc_autopilot.c
+++ b/sw/airborne/csc/csc_autopilot.c
@@ -32,26 +32,44 @@
#include "pprz_algebra_float.h"
#include "string.h"
#include "radio_control.h"
+#include "pwm_input.h"
struct control_gains csc_gains;
struct control_reference csc_reference;
struct control_reference csc_errors;
struct control_trims csc_trims;
+float csc_vane_angle;
+float csc_vane_angle_offset = 180;
static const int xsens_id = 0;
float csc_yaw_weight;
+float csc_yaw_rudder;
+float csc_yaw_aileron;
+float csc_yaw_deadband;
+float csc_yaw_setpoint_rate;
+float csc_yaw_setpoint_range;
+
+#define PWM_INPUT_COUNTS_PER_REV 61358.
+static void update_vane_angle( void )
+{
+ csc_vane_angle = RadOfDeg( 360. * pwm_input_duration / PWM_INPUT_COUNTS_PER_REV) - RadOfDeg(csc_vane_angle_offset);
+}
void csc_ap_init( void )
{
- csc_gains.roll_kp = 2700;
- csc_gains.roll_kd = 2500;
- csc_gains.roll_ki = 0;
+ csc_gains.roll_kp = 4218;
+ csc_gains.roll_kd = 1000;
+ csc_gains.roll_ki = 50;
csc_gains.pitch_kp = 0;
csc_gains.pitch_kd = 0;
csc_gains.pitch_ki = 0;
- csc_gains.yaw_kp = 8700;
- csc_gains.yaw_kd = 900;
- csc_gains.yaw_ki = 30;
+ csc_gains.yaw_kp = 5000;
+ csc_gains.yaw_kd = 800;
+ csc_gains.yaw_ki = 10;
+ csc_yaw_weight = 0;
+ csc_yaw_rudder = 0.33;
+ csc_yaw_aileron = 0;
+ csc_yaw_deadband = 1.00;
csc_trims.elevator = 1800;
csc_trims.aileron = -60;
@@ -121,15 +139,26 @@ static void calculate_errors(struct control_reference *errors)
errors->eulers_i.phi += xsens_eulers.phi;
errors->eulers_i.theta += xsens_eulers.theta;
errors->eulers_i.psi += xsens_eulers.psi;
+
+ float yaw_deadband = RadOfDeg(csc_yaw_deadband);
+
+ if (errors->eulers.psi <= yaw_deadband && errors->eulers.psi >= -yaw_deadband) {
+ errors->eulers.psi = 0;
+ } else if (errors->eulers.psi <= yaw_deadband*2 && errors->eulers.psi >= -yaw_deadband*2) {
+ if (errors->eulers.psi < 0) {
+ errors->eulers.psi = (errors->eulers.psi + yaw_deadband)*2;
+ } else
+ errors->eulers.psi = (errors->eulers.psi - yaw_deadband)*2;
+ }
bound_ierror(csc_gains.roll_ki, &errors->eulers_i.phi);
bound_ierror(csc_gains.pitch_ki, &errors->eulers_i.theta);
bound_ierror(csc_gains.yaw_ki, &errors->eulers_i.psi);
}
-static void calculate_reference(struct control_reference *reference)
+static void calculate_reference(struct control_reference *reference, int time)
{
- reference->eulers.psi = M_PI / 6.0 * rc_values[RADIO_YAW];
+ reference->eulers.psi = M_PI / 6.0 * rc_values[RADIO_YAW] + 100*csc_yaw_setpoint_range*sin(0.01*time*csc_yaw_setpoint_rate);
reference->eulers.theta = M_PI / 6.0 * rc_values[RADIO_PITCH];
// Mix reference command with yaw reference command to prevent
// fighting ourselves
@@ -140,11 +169,12 @@ static void calculate_reference(struct control_reference *reference)
reference->eulers.psi /= MAX_PPRZ;
}
-void csc_ap_periodic( void )
+void csc_ap_periodic(int time)
{
static int counter = 0;
- calculate_reference(&csc_reference);
+ calculate_reference(&csc_reference, time);
calculate_errors(&csc_errors);
+ update_vane_angle();
commands[COMMAND_ROLL] = -csc_gains.roll_kp * (csc_errors.eulers.phi + csc_errors.eulers.psi * csc_yaw_weight)
+ csc_gains.roll_kd * (csc_errors.rates.p + csc_errors.rates.r * csc_yaw_weight)
@@ -156,10 +186,15 @@ void csc_ap_periodic( void )
- csc_gains.pitch_ki * csc_errors.eulers_i.theta;
commands[COMMAND_PITCH] += csc_trims.elevator;
- commands[COMMAND_ROLL] += -csc_gains.yaw_kp * csc_errors.eulers.psi
+ commands[COMMAND_ROLL] += (-csc_gains.yaw_kp * csc_errors.eulers.psi
- csc_gains.yaw_kd * csc_errors.rates.r
- - csc_gains.yaw_ki * csc_errors.eulers_i.psi;
+ - csc_gains.yaw_ki * csc_errors.eulers_i.psi) * csc_yaw_aileron;
+
commands[COMMAND_YAW] = csc_trims.rudder;
+
+ commands[COMMAND_YAW] += (-csc_gains.yaw_kp * csc_errors.eulers.psi
+ - csc_gains.yaw_kd * csc_errors.rates.r
+ - csc_gains.yaw_ki * csc_errors.eulers_i.psi) * csc_yaw_rudder;
}
void csc_ap_clear_ierrors( void )
diff --git a/sw/airborne/csc/csc_autopilot.h b/sw/airborne/csc/csc_autopilot.h
index a7f54228aa..266e6a1798 100644
--- a/sw/airborne/csc/csc_autopilot.h
+++ b/sw/airborne/csc/csc_autopilot.h
@@ -56,16 +56,24 @@ struct control_trims {
extern struct control_gains csc_gains;
extern struct control_reference csc_reference;
extern struct control_trims csc_trims;
+extern float csc_vane_angle;
+extern float csc_vane_angle_offset;
extern float csc_yaw_weight;
+extern float csc_yaw_rudder;
+extern float csc_yaw_aileron;
+extern float csc_yaw_deadband;
+extern float csc_yaw_setpoint_rate;
+extern float csc_yaw_setpoint_range;
void csc_autopilot_set_roll_ki(float ki);
void csc_autopilot_set_pitch_ki(float ki);
void csc_autopilot_set_yaw_ki(float ki);
void csc_ap_init( void );
-void csc_ap_periodic (void );
+void csc_ap_periodic (int time);
void csc_ap_set_trims (void );
void csc_ap_clear_ierrors (void );
+#define PERIODIC_SEND_VANE_SENSOR() DOWNLINK_SEND_VANE_SENSOR(&csc_vane_angle)
#endif
diff --git a/sw/airborne/csc/csc_can.c b/sw/airborne/csc/csc_can.c
index 19b1de6fff..ac0c4732d4 100644
--- a/sw/airborne/csc/csc_can.c
+++ b/sw/airborne/csc/csc_can.c
@@ -153,7 +153,6 @@ void csc_can_event(void)
{
#ifdef USE_CAN1
if (can1_msg_received) {
- LED_ON(CAN_LED);
can1_callback(&can1_rx_msg);
can1_msg_received = FALSE;
}
diff --git a/sw/airborne/csc/csc_telemetry.h b/sw/airborne/csc/csc_telemetry.h
index 422e54899a..abeed4812c 100644
--- a/sw/airborne/csc/csc_telemetry.h
+++ b/sw/airborne/csc/csc_telemetry.h
@@ -34,29 +34,39 @@
#define PERIODIC_SEND_ALIVE() DOWNLINK_SEND_ALIVE(16, MD5SUM)
+#define PERIODIC_SEND_DOWNLINK() { \
+ static uint16_t last; \
+ uint16_t rate = (downlink_nb_bytes - last) / PERIOD_DOWNLINK_0; \
+ last = downlink_nb_bytes; \
+ DOWNLINK_SEND_DOWNLINK(&downlink_nb_ovrn, &rate, &downlink_nb_msgs); \
+}
+
+
+#ifdef PROPS_NB
+
+#include "mercury_ap.h"
+#define PERIODIC_SEND_MERCURY_PROPS() DOWNLINK_SEND_MERCURY_PROPS(&(mixed_commands[0]),&(mixed_commands[1]),&(mixed_commands[2]),&(mixed_commands[3]))
+
+#endif /* PROPS_NB */
#ifdef COMMANDS_NB
-
#include "commands.h"
-
#define PERIODIC_SEND_COMMANDS() DOWNLINK_SEND_COMMANDS(COMMANDS_NB, commands)
-
-#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 */
+#endif /* COMMANDS_NB */
+#define PERIODIC_SEND_SIMPLE_COMMANDS() DOWNLINK_SEND_SIMPLE_COMMANDS(&commands[0], &commands[1], &commands[2])
#ifdef SERVOS_NB
#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 */
+
+
extern uint8_t telemetry_mode_Ap;
#endif /* CSC_TELEMETRY_H */
diff --git a/sw/airborne/csc/mercury_ap.c b/sw/airborne/csc/mercury_ap.c
index 1a864abd30..ca9161f65e 100644
--- a/sw/airborne/csc/mercury_ap.c
+++ b/sw/airborne/csc/mercury_ap.c
@@ -52,11 +52,13 @@ uint8_t booz2_autopilot_tol;
uint32_t booz2_autopilot_motors_on_counter;
uint32_t booz2_autopilot_in_flight_counter;
+uint16_t mercury_yaw_servo_gain;
-#define BOOZ2_AUTOPILOT_MOTOR_ON_TIME 40
+
+#define BOOZ2_AUTOPILOT_MOTOR_ON_TIME (512/4)
#define BOOZ2_AUTOPILOT_IN_FLIGHT_TIME 40
-#define BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD (MAX_PPRZ / 20)
-#define BOOZ2_AUTOPILOT_YAW_TRESHOLD (MAX_PPRZ * 19 / 20)
+#define BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD (MAX_PPRZ * -14 / 20)
+#define BOOZ2_AUTOPILOT_YAW_TRESHOLD (MAX_PPRZ * 14 / 20)
void csc_ap_init(void) {
@@ -101,31 +103,18 @@ void csc_ap_init(void) {
}
#define BOOZ2_AUTOPILOT_CHECK_MOTORS_ON() { \
- if (booz2_autopilot_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 ( booz2_autopilot_motors_on_counter > 0) { \
- booz2_autopilot_motors_on_counter--; \
- if (booz2_autopilot_motors_on_counter == 0) \
- booz2_autopilot_motors_on = FALSE; \
- } \
- } \
- else { /* sticks not in the corner */ \
- booz2_autopilot_motors_on_counter = BOOZ2_AUTOPILOT_MOTOR_ON_TIME; \
- } \
- } \
- else { /* motors off */ \
- 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 ( 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) \
- booz2_autopilot_motors_on = TRUE; \
- } \
- } \
- else { \
+ 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){ \
+ booz2_autopilot_motors_on = TRUE; \
+ booz2_autopilot_in_flight_counter += BOOZ2_AUTOPILOT_MOTOR_ON_TIME; \
+ } \
+ } \
+ } else { \
booz2_autopilot_motors_on_counter = 0; \
} \
} \
@@ -147,39 +136,35 @@ const pprz_t csc_ap_commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE;
pprz_t mixed_commands[PROPS_NB];
-void csc_ap_periodic(int8_t _in_flight, int8_t _motors_on) {
+void csc_ap_periodic(uint8_t _in_flight, uint8_t kill) {
// RunOnceEvery(50, nav_periodic_task_10Hz())
- booz2_autopilot_motors_on = _motors_on;
+ BOOZ2_AUTOPILOT_CHECK_MOTORS_ON();
+ 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);
booz2_guidance_v_run(booz2_autopilot_in_flight);
- /* if ( !booz2_autopilot_motors_on ){ */
-/* if( booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE || */
-/* booz2_autopilot_mode == BOOZ2_AP_MODE_KILL ) { */
-/* CscSetCommands(csc_ap_commands_failsafe, */
-/* booz2_autopilot_in_flight, booz2_autopilot_motors_on); */
+ booz2_stabilization_cmd[COMMAND_THRUST] = (int32_t)rc_values[RADIO_THROTTLE] * 105 / 7200 + 95;
+
+
+ CscSetCommands(booz2_stabilization_cmd,
+ booz2_autopilot_in_flight,booz2_autopilot_motors_on);
-/* } else { */
- booz2_stabilization_attitude_read_rc(booz2_autopilot_in_flight);
- booz2_stabilization_cmd[COMMAND_THRUST] = (int32_t)rc_values[RADIO_THROTTLE] * 200 / MAX_PPRZ + 118;
- CscSetCommands(booz2_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]);
- props_commit();
-
-
- Actuator(SERVO_S1) = (SERVO_S1_MAX+SERVO_S1_MIN)/2 +((SERVO_S1_MAX-SERVO_S1_MIN)*rc_values[RADIO_YAW])/2/7200;
- Actuator(SERVO_S2) = (SERVO_S2_MAX+SERVO_S2_MIN)/2 +((SERVO_S2_MAX-SERVO_S2_MIN)*rc_values[RADIO_YAW])/2/7200;
- ActuatorsCommit();
+ 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]);
+ props_commit();
+
+
+ MERCURY_SURFACES_SUPERVISION_RUN(Actuator,
+ booz2_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 8f54e19271..2924ead6d4 100644
--- a/sw/airborne/csc/mercury_ap.h
+++ b/sw/airborne/csc/mercury_ap.h
@@ -25,9 +25,12 @@
#ifndef __MERCURY_AP_H__
#define __MERCURY_AP_H__
+#include "airframe.h"
+
+extern pprz_t mixed_commands[PROPS_NB];
void csc_ap_init( void );
-void csc_ap_periodic (int8_t flight, int8_t motors );
+void csc_ap_periodic (uint8_t _in_flight, uint8_t kill);
#endif
diff --git a/sw/airborne/csc/mercury_csc_main.c b/sw/airborne/csc/mercury_csc_main.c
index 0e1fa63843..2faf743780 100644
--- a/sw/airborne/csc/mercury_csc_main.c
+++ b/sw/airborne/csc/mercury_csc_main.c
@@ -44,7 +44,6 @@
#include "downlink.h"
-#include "csc_throttle.h"
#include "csc_adc.h"
#include "csc_rc_spektrum.h"
@@ -97,7 +96,6 @@ static void csc_main_init( void ) {
#endif
motors_init();
- csc_throttle_init();
int_enable();
}
@@ -128,7 +126,6 @@ static void csc_main_periodic( void ) {
static void csc_main_event( void ) {
csc_can_event();
- csc_throttle_event_task();
#ifdef SPEKTRUM_LINK
spektrum_event_task();
#endif
@@ -168,10 +165,7 @@ static void on_servo_cmd(struct CscServoCmd *cmd)
static 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;
- csc_throttle_send_msg(throttle_id, msg->cmd_id, msg->arg1, msg->arg2);
}
int main( void ) {
diff --git a/sw/airborne/csc/mercury_main.c b/sw/airborne/csc/mercury_main.c
index 94a4b8b5e9..5fe581c17f 100644
--- a/sw/airborne/csc/mercury_main.c
+++ b/sw/airborne/csc/mercury_main.c
@@ -159,7 +159,7 @@ static inline void csc_main_periodic( void )
baro_scp_periodic();
csc_ap_periodic(pprz_mode == PPRZ_MODE_IN_FLIGHT,
- pprz_mode > PPRZ_MODE_MOTORS_OFF && booz_ahrs_aligner.status == BOOZ_AHRS_ALIGNER_LOCKED);
+ !(pprz_mode > PPRZ_MODE_MOTORS_OFF && booz_ahrs_aligner.status == BOOZ_AHRS_ALIGNER_LOCKED));
}
diff --git a/sw/airborne/csc/mercury_supervision.h b/sw/airborne/csc/mercury_supervision.h
index 56e49bf27a..68aedce137 100644
--- a/sw/airborne/csc/mercury_supervision.h
+++ b/sw/airborne/csc/mercury_supervision.h
@@ -3,6 +3,11 @@
#include "airframe.h"
+extern pprz_t mercury_supervision_yaw_servo_gain;
+extern pprz_t mercury_supervision_yaw_motor_gain;
+extern pprz_t mercury_supervision_yaw_comp_slope;
+extern pprz_t mercury_supervision_yaw_comp_offset;
+
/* #if defined SUPERVISION_FRONT_ROTOR_CW */
/* #define TRIM_FRONT ( SUPERVISION_TRIM_E-SUPERVISION_TRIM_R) */
/* #define TRIM_RIGHT (-SUPERVISION_TRIM_A+SUPERVISION_TRIM_R) */
@@ -65,10 +70,23 @@
Bound(_mot_cmd[PROP_UPPER_RIGHT] , SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \
}
+#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; \
+ } 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); \
+ _actuators(SERVO_S1) = (SERVO_S1_MAX+SERVO_S1_MIN)/2 - \
+ ((SERVO_S1_MAX-SERVO_S1_MIN)*bndcmd)/2/400; \
+ _actuators(SERVO_S2) = (SERVO_S2_MAX+SERVO_S2_MIN)/2 - \
+ ((SERVO_S2_MAX-SERVO_S2_MIN)*bndcmd)/2/400; \
+ } \
+ }
#define BOOZ2_SUPERVISION_RUN(_out, _in,_motors_on) { \
if (_motors_on) { \
- SUPERVISION_MIX(_out, _in[COMMAND_ROLL], _in[COMMAND_PITCH], _in[COMMAND_YAW], _in[COMMAND_THRUST]); \
+ SUPERVISION_MIX(_out, _in[COMMAND_ROLL], _in[COMMAND_PITCH], (mercury_supervision_yaw_motor_gain*_in[COMMAND_YAW])/20, _in[COMMAND_THRUST]); \
pprz_t min_mot; \
SUPERVISION_FIND_MIN_MOTOR(_out, min_mot); \
if (min_mot < SUPERVISION_MIN_MOTOR) { \