rate sp in degrees/s

paparazzi · Dec 15, 2015 · 2a6b981 · 2a6b981
1 parent 1951056
commit 2a6b981
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 9 deletions.
diff --git a/conf/airframes/TUDELFT/tudelft_heli450.xml b/conf/airframes/TUDELFT/tudelft_heli450.xml
@@ -115,9 +115,9 @@
 
   <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
     <!-- setpoints -->
-    <define name="SP_MAX_P" value="10000"/>
-    <define name="SP_MAX_Q" value="10000"/>
-    <define name="SP_MAX_R" value="10000"/>
+    <define name="SP_MAX_P" unit="deg/s" value="280"/>
+    <define name="SP_MAX_Q" unit="deg/s" value="280"/>
+    <define name="SP_MAX_R" unit="deg/s" value="140"/>
     <define name="DEADBAND_P" value="20"/>
     <define name="DEADBAND_Q" value="20"/>
     <define name="DEADBAND_R" value="200"/>

diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
@@ -137,19 +137,19 @@ void stabilization_rate_read_rc(void)
 {
 
   if (ROLL_RATE_DEADBAND_EXCEEDED()) {
-    stabilization_rate_sp.p = (int32_t)radio_control.values[RADIO_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ;
+    stabilization_rate_sp.p = (int32_t)radio_control.values[RADIO_ROLL] * RATE_BFP_OF_REAL(STABILIZATION_RATE_SP_MAX_P) / MAX_PPRZ;
   } else {
     stabilization_rate_sp.p = 0;
   }
 
   if (PITCH_RATE_DEADBAND_EXCEEDED()) {
-    stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_PITCH] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ;
+    stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_PITCH] * RATE_BFP_OF_REAL(STABILIZATION_RATE_SP_MAX_Q) / MAX_PPRZ;
   } else {
     stabilization_rate_sp.q = 0;
   }
 
   if (YAW_RATE_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
-    stabilization_rate_sp.r = (int32_t)radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ;
+    stabilization_rate_sp.r = (int32_t)radio_control.values[RADIO_YAW] * RATE_BFP_OF_REAL(STABILIZATION_RATE_SP_MAX_R) / MAX_PPRZ;
   } else {
     stabilization_rate_sp.r = 0;
   }
@@ -160,19 +160,19 @@ 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;
+    stabilization_rate_sp.r = (int32_t) - radio_control.values[RADIO_ROLL] * RATE_BFP_OF_REAL(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;
+    stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_PITCH] * RATE_BFP_OF_REAL(STABILIZATION_RATE_SP_MAX_Q) / MAX_PPRZ;
   } else {
     stabilization_rate_sp.q = 0;
   }
 
   if (YAW_RATE_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
-    stabilization_rate_sp.p = (int32_t)radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ;
+    stabilization_rate_sp.p = (int32_t)radio_control.values[RADIO_YAW] * RATE_BFP_OF_REAL(STABILIZATION_RATE_SP_MAX_R) / MAX_PPRZ;
   } else {
     stabilization_rate_sp.p = 0;
   }