Care Free mode

FlyTheThings · Mar 8, 2013 · 7636a96 · 7636a96
1 parent 4733bc4
commit 7636a96
Show file tree

Hide file tree

Showing 7 changed files with 23 additions and 5 deletions.
diff --git a/conf/airframes/esden/gain_scheduling_example.xml b/conf/airframes/esden/gain_scheduling_example.xml
@@ -210,8 +210,6 @@
       <subsystem name="radio_control" type="ppm">
       	<configure name="RADIO_CONTROL_PPM_PIN" value="UART1_RX"/>
       </subsystem>
-
-      <configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
     </target>
 
     <target name="nps" board="pc">

diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c
@@ -150,6 +150,10 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
     case AP_MODE_RATE_Z_HOLD:
       guidance_h_mode_changed(GUIDANCE_H_MODE_RATE);
       break;
+    case AP_MODE_CARE_FREE:
+      //Take the current psi as the reference for pitch and roll
+      care_free_heading = stateGetNedToBodyEulers_f()->psi;
+    case AP_MODE_ATTITUDE_RC_CLIMB:
     case AP_MODE_ATTITUDE_DIRECT:
     case AP_MODE_ATTITUDE_CLIMB:
     case AP_MODE_ATTITUDE_Z_HOLD:
@@ -181,6 +185,7 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
     case AP_MODE_RATE_DIRECT:
     case AP_MODE_ATTITUDE_DIRECT:
     case AP_MODE_HOVER_DIRECT:
+    case AP_MODE_CARE_FREE:
       guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT);
       break;
     case AP_MODE_RATE_RC_CLIMB:

diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h
@@ -49,8 +49,8 @@
 #define AP_MODE_HOVER_CLIMB       10
 #define AP_MODE_HOVER_Z_HOLD      11
 #define AP_MODE_NAV               12
-#define AP_MODE_RC_DIRECT	  13	// Safety Pilot Direct Commands for helicopter direct control: appropriately chosen as mode "13"
-
+#define AP_MODE_RC_DIRECT         13	// Safety Pilot Direct Commands for helicopter direct control: appropriately chosen as mode "13"
+#define AP_MODE_CARE_FREE         14
 
 extern uint8_t autopilot_mode;
 extern uint8_t autopilot_mode_auto2;

diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
@@ -24,6 +24,9 @@
 
 
 #include STABILIZATION_ATTITUDE_TYPE_H
+
+extern float care_free_heading;
+
 extern void stabilization_attitude_init(void);
 extern void stabilization_attitude_read_rc(bool_t in_flight);
 extern void stabilization_attitude_enter(void);

diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
@@ -58,6 +58,8 @@ struct Int32Eulers stabilization_att_sum_err;
 int32_t stabilization_att_fb_cmd[COMMANDS_NB];
 int32_t stabilization_att_ff_cmd[COMMANDS_NB];
 
+float care_free_heading = 0;
+
 #define IERROR_SCALE 1024
 #define GAIN_PRESCALER_FF 48
 #define GAIN_PRESCALER_P 48

diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h
@@ -33,6 +33,7 @@
 
 #include "subsystems/radio_control.h"
 #include "state.h"
+#include "firmwares/rotorcraft/autopilot.h"
 
 #if defined STABILIZATION_ATTITUDE_TYPE_INT
 #define SP_MAX_PHI     (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI)
@@ -187,7 +188,15 @@ static inline void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQu
   /* get current heading */
   const struct FloatVect3 zaxis = {0., 0., 1.};
   struct FloatQuat q_yaw;
-  FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, stateGetNedToBodyEulers_f()->psi);
+
+  //Care Free mode
+  if(autopilot_mode == AP_MODE_CARE_FREE) {
+    //care_free_heading has been set to current psi when entering care free mode.
+    FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, care_free_heading);
+  }
+  else {
+    FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, stateGetNedToBodyEulers_f()->psi);
+  }
 
   /* roll/pitch commands applied to to current heading */
   struct FloatQuat q_rp_sp;

diff --git a/sw/airborne/modules/gain_scheduling/gain_scheduling.c b/sw/airborne/modules/gain_scheduling/gain_scheduling.c
@@ -27,6 +27,7 @@
 
 //Include for scheduling on transition_status
 #include "firmwares/rotorcraft/guidance/guidance_h.h"
+#include "firmwares/rotorcraft/stabilization.h"
 
 // #include "state.h"
 #include "math/pprz_algebra_int.h"