diff --git a/conf/airframes/esden/calib/asp22-019.xml b/conf/airframes/esden/calib/asp22-019.xml new file mode 100644 index 0000000000..c3eb571d5b --- /dev/null +++ b/conf/airframes/esden/calib/asp22-019.xml @@ -0,0 +1,17 @@ + +
+ + + + + + + + + + + + + +
+
diff --git a/conf/airframes/esden/qs_asp22.xml b/conf/airframes/esden/qs_asp22.xml index 583004514a..a37d1d6f2c 100644 --- a/conf/airframes/esden/qs_asp22.xml +++ b/conf/airframes/esden/qs_asp22.xml @@ -175,7 +175,7 @@ - +
@@ -195,7 +195,7 @@ - +
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 376efbf4c5..4d07646f7a 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -164,7 +164,11 @@ void guidance_h_read_rc(bool_t in_flight) { break; case GUIDANCE_H_MODE_RATE: +#if SWITCH_STICKS_FOR_RATE_CONTROL + stabilization_rate_read_rc_switched_sticks(); +#else stabilization_rate_read_rc(); +#endif break; case GUIDANCE_H_MODE_FORWARD: diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c index b1f39edc4f..486ca4deb9 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c @@ -164,6 +164,28 @@ void stabilization_rate_read_rc( void ) { INT_RATES_LSHIFT(stabilization_rate_sp, stabilization_rate_sp, REF_FRAC - INT32_RATE_FRAC); } +//Read rc with roll and yaw sitcks switched if the default orientation is vertical but airplane sticks are desired +void stabilization_rate_read_rc_switched_sticks( void ) { + + if(ROLL_RATE_DEADBAND_EXCEEDED()) + stabilization_rate_sp.r = (int32_t) -radio_control.values[RADIO_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ; + else + stabilization_rate_sp.r = 0; + + if(PITCH_RATE_DEADBAND_EXCEEDED()) + stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_PITCH] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ; + else + stabilization_rate_sp.q = 0; + + if(YAW_RATE_DEADBAND_EXCEEDED()) + stabilization_rate_sp.p = (int32_t)radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ; + else + stabilization_rate_sp.p = 0; + + // Setpoint at ref resolution + INT_RATES_LSHIFT(stabilization_rate_sp, stabilization_rate_sp, REF_FRAC - INT32_RATE_FRAC); +} + void stabilization_rate_enter(void) { RATES_COPY(stabilization_rate_ref, stabilization_rate_sp); INT_RATES_ZERO(stabilization_rate_sum_err); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.h index 41a6eaf99b..5263e4dd31 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.h @@ -32,6 +32,7 @@ extern void stabilization_rate_init(void); extern void stabilization_rate_read_rc(void); +extern void stabilization_rate_read_rc_switched_sticks(void); extern void stabilization_rate_run(bool_t in_flight); extern void stabilization_rate_enter(void);