Merge branch 'care_free_mode'

New "care free" mode that keeps the yaw ref pointing in initial direction:
AP_MODE_CARE_FREE_DIRECT: otherwise like the normal ATTITUDE_DIRECT mode.
CARE_FREE is implemented as a horizontal submode: GUIDANCE_H_MODE_CARE_FREE
so it could be combined with other vertical modes as well...

Other changes:
- move read_rc_setpoint_* functions to c file
- added missing modes to settings/telemetry
- add stabilization float_euler subsystem
- use normal stabilization subsystem in NPS instead of hardcoded euler

closes #390 and closes #391

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">

conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile

@@ -111,39 +111,6 @@ nps.srcs += $(SRC_FIRMWARE)/stabilization.c
 nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_rate.c
 nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_none.c
 
-
-NUM_TYPE=integer
-#NUM_TYPE=float
-
-STAB_TYPE=euler
-#STAB_TYPE=quaternion
-
-ifeq ($(NUM_TYPE), integer)
-  nps.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_INT
-  nps.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_int.h\"
-  ifeq ($(STAB_TYPE), euler)
-    nps.CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_euler_int.h\"
-    nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_int.c
-    nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_int.c
-  else ifeq ($(STAB_TYPE), quaternion)
-    nps.CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_quat_int.h\"
-    nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c
-    nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c
-  endif
-else ifeq ($(NUM_TYPE), float)
-  nps.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_FLOAT
-  nps.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_float.h\"
-  ifeq ($(STAB_TYPE), euler)
-    nps.CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_euler_float.h\"
-    nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_float.c
-    nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_float.c
-  else ifeq ($(STAB_TYPE), quaternion)
-    nps.CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_quat_float.h\"
-    nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_float.c
-    nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_float.c
-  endif
-endif
-
 nps.CFLAGS += -DUSE_NAVIGATION
 nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c
 nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c

conf/firmwares/subsystems/rotorcraft/stabilization_euler.makefile

@@ -1,5 +1,12 @@
-ap.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_INT
-ap.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_int.h\"
-ap.CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_euler_int.h\"
-ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_int.c
-ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_int.c
+STAB_ATT_CFLAGS  = -DSTABILIZATION_ATTITUDE_TYPE_INT
+STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_int.h\"
+STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_euler_int.h\"
+STAB_ATT_SRCS  = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_int.c
+STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_int.c
+STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c
+
+ap.CFLAGS += $(STAB_ATT_CFLAGS)
+ap.srcs += $(STAB_ATT_SRCS)
+
+nps.CFLAGS += $(STAB_ATT_CFLAGS)
+nps.srcs += $(STAB_ATT_SRCS)

conf/firmwares/subsystems/rotorcraft/stabilization_float_euler.makefile

@@ -0,0 +1,12 @@
+STAB_ATT_CFLAGS  = -DSTABILIZATION_ATTITUDE_TYPE_FLOAT
+STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_float.h\"
+STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_euler_float.h\"
+STAB_ATT_SRCS  = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_float.c
+STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_float.c
+STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c
+
+ap.CFLAGS += $(STAB_ATT_CFLAGS)
+ap.srcs += $(STAB_ATT_SRCS)
+
+nps.CFLAGS += $(STAB_ATT_CFLAGS)
+nps.srcs += $(STAB_ATT_SRCS)

conf/firmwares/subsystems/rotorcraft/stabilization_float_quat.makefile

@@ -1,6 +1,13 @@
-ap.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_FLOAT
-ap.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_QUAT
-ap.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_float.h\"
-ap.CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_quat_float.h\"
-ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_float.c
-ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_float.c
+STAB_ATT_CFLAGS  = -DSTABILIZATION_ATTITUDE_TYPE_FLOAT
+STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_QUAT
+STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_float.h\"
+STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_quat_float.h\"
+STAB_ATT_SRCS  = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_float.c
+STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_float.c
+STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c
+
+ap.CFLAGS += $(STAB_ATT_CFLAGS)
+ap.srcs += $(STAB_ATT_SRCS)
+
+nps.CFLAGS += $(STAB_ATT_CFLAGS)
+nps.srcs += $(STAB_ATT_SRCS)

conf/firmwares/subsystems/rotorcraft/stabilization_int_quat.makefile

@@ -1,6 +1,13 @@
-ap.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_INT
-ap.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_QUAT
-ap.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_int.h\"
-ap.CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_quat_int.h\"
-ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c
-ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c
+STAB_ATT_CFLAGS  = -DSTABILIZATION_ATTITUDE_TYPE_INT
+STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_QUAT
+STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_int.h\"
+STAB_ATT_CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_quat_int.h\"
+STAB_ATT_SRCS  = $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c
+STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c
+STAB_ATT_SRCS += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_rc_setpoint.c
+
+ap.CFLAGS += $(STAB_ATT_CFLAGS)
+ap.srcs += $(STAB_ATT_SRCS)
+
+nps.CFLAGS += $(STAB_ATT_CFLAGS)
+nps.srcs += $(STAB_ATT_SRCS)

conf/messages.xml

@@ -1821,10 +1821,10 @@
     <field name="rc_status"    type="uint8" values="OK|LOST|REALLY_LOST"/>
     <field name="frame_rate"   type="uint8" unit="Hz"/>
     <field name="gps_status"   type="uint8" values="NO_FIX|NA|NA|3Dfix"/>
-    <field name="ap_mode"      type="uint8" values="FAILSAFE|KILL|RATE_DIRECT|ATTITUDE_DIRECT|RATE_RC_CLIMB|ATTITUDE_RC_CLIMB|ATTITUDE_CLIMB|RATE_Z_HOLD|ATTITUDE_Z_HOLD|HOVER_DIRECT|HOVER_CLIMB|HOVER_Z_HOLD|NAV"/>
+    <field name="ap_mode"      type="uint8" values="FAILSAFE|KILL|RATE_DIRECT|ATTITUDE_DIRECT|RATE_RC_CLIMB|ATTITUDE_RC_CLIMB|ATTITUDE_CLIMB|RATE_Z_HOLD|ATTITUDE_Z_HOLD|HOVER_DIRECT|HOVER_CLIMB|HOVER_Z_HOLD|NAV|RC_DIRECT|CARE_FREE"/>
     <field name="ap_in_flight" type="uint8" values="ON_GROUND|IN_FLIGHT"/>
     <field name="ap_motors_on" type="uint8" values="MOTORS_OFF|MOTORS_ON"/>
-    <field name="ap_h_mode"    type="uint8" values="KILL|RATE|ATTITUDE|HOVER|NAV"/>
+    <field name="ap_h_mode"    type="uint8" values="KILL|RATE|ATTITUDE|HOVER|NAV|CF"/>
     <field name="ap_v_mode"    type="uint8" values="KILL|RC_DIRECT|RC_CLIMB|CLIMB|HOVER|NAV"/>
     <field name="vsupply"      type="uint16" unit="decivolt"/>
     <field name="cpu_time"     type="uint16" unit="s"/>

conf/settings/rotorcraft_basic.xml

@@ -4,7 +4,7 @@
   <dl_settings>
 
     <dl_settings NAME="System">
-       <dl_setting var="autopilot_mode_auto2" min="0" step="1" max="13" module="autopilot" shortname="auto2" values="Fail|Kill|Rate|Att|Rate_rcC|Att_rcC|Att_C|Rate_Z|Att_Z|Hover|Hover_C|Hover_Z|Nav|RC_D"/>
+       <dl_setting var="autopilot_mode_auto2" min="0" step="1" max="14" module="autopilot" shortname="auto2" values="Fail|Kill|Rate|Att|Rate_rcC|Att_rcC|Att_C|Rate_Z|Att_Z|Hover|Hover_C|Hover_Z|Nav|RC_D|CareFree"/>
       <dl_setting var="kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
       <dl_setting var="autopilot_power_switch" min="0" step="1" max="1" module="autopilot" values="OFF|ON" handler="SetPowerSwitch">
         <strip_button name="POWER ON" icon="on.png" value="1" group="power_switch"/>

sw/airborne/firmwares/rotorcraft/autopilot.c

@@ -130,77 +130,82 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
   if (new_autopilot_mode != autopilot_mode) {
     /* horizontal mode */
     switch (new_autopilot_mode) {
-    case AP_MODE_FAILSAFE:
+      case AP_MODE_FAILSAFE:
 #ifndef KILL_AS_FAILSAFE
-      stab_att_sp_euler.phi = 0;
-      stab_att_sp_euler.theta = 0;
-      guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
-      break;
+        stab_att_sp_euler.phi = 0;
+        stab_att_sp_euler.theta = 0;
+        guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
+        break;
 #endif
-    case AP_MODE_KILL:
-      autopilot_set_motors_on(FALSE);
-      autopilot_in_flight = FALSE;
-      autopilot_in_flight_counter = 0;
-      guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
-      break;
-    case AP_MODE_RC_DIRECT:
-      guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT);
-      break;
-    case AP_MODE_RATE_DIRECT:
-    case AP_MODE_RATE_Z_HOLD:
-      guidance_h_mode_changed(GUIDANCE_H_MODE_RATE);
-      break;
-    case AP_MODE_ATTITUDE_DIRECT:
-    case AP_MODE_ATTITUDE_CLIMB:
-    case AP_MODE_ATTITUDE_Z_HOLD:
-      guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
-      break;
-    case AP_MODE_HOVER_DIRECT:
-    case AP_MODE_HOVER_CLIMB:
-    case AP_MODE_HOVER_Z_HOLD:
-      guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER);
-      break;
-    case AP_MODE_NAV:
-      guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
-      break;
-    default:
-      break;
+      case AP_MODE_KILL:
+        autopilot_set_motors_on(FALSE);
+        autopilot_in_flight = FALSE;
+        autopilot_in_flight_counter = 0;
+        guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
+        break;
+      case AP_MODE_RC_DIRECT:
+        guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT);
+        break;
+      case AP_MODE_RATE_DIRECT:
+      case AP_MODE_RATE_Z_HOLD:
+        guidance_h_mode_changed(GUIDANCE_H_MODE_RATE);
+        break;
+      case AP_MODE_ATTITUDE_RC_CLIMB:
+      case AP_MODE_ATTITUDE_DIRECT:
+      case AP_MODE_ATTITUDE_CLIMB:
+      case AP_MODE_ATTITUDE_Z_HOLD:
+        guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
+        break;
+      case AP_MODE_CARE_FREE_DIRECT:
+        guidance_h_mode_changed(GUIDANCE_H_MODE_CARE_FREE);
+        break;
+      case AP_MODE_HOVER_DIRECT:
+      case AP_MODE_HOVER_CLIMB:
+      case AP_MODE_HOVER_Z_HOLD:
+        guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER);
+        break;
+      case AP_MODE_NAV:
+        guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
+        break;
+      default:
+        break;
     }
     /* vertical mode */
     switch (new_autopilot_mode) {
-    case AP_MODE_FAILSAFE:
+      case AP_MODE_FAILSAFE:
 #ifndef KILL_AS_FAILSAFE
-      guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5);
-      guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
-      break;
+        guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5);
+        guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
+        break;
 #endif
-    case AP_MODE_KILL:
-      guidance_v_mode_changed(GUIDANCE_V_MODE_KILL);
-      break;
-    case AP_MODE_RC_DIRECT:
-    case AP_MODE_RATE_DIRECT:
-    case AP_MODE_ATTITUDE_DIRECT:
-    case AP_MODE_HOVER_DIRECT:
-      guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT);
-      break;
-    case AP_MODE_RATE_RC_CLIMB:
-    case AP_MODE_ATTITUDE_RC_CLIMB:
-      guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB);
-      break;
-    case AP_MODE_ATTITUDE_CLIMB:
-    case AP_MODE_HOVER_CLIMB:
-      guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
-      break;
-    case AP_MODE_RATE_Z_HOLD:
-    case AP_MODE_ATTITUDE_Z_HOLD:
-    case AP_MODE_HOVER_Z_HOLD:
-      guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER);
-      break;
-    case AP_MODE_NAV:
-      guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
-      break;
-    default:
-      break;
+      case AP_MODE_KILL:
+        guidance_v_mode_changed(GUIDANCE_V_MODE_KILL);
+        break;
+      case AP_MODE_RC_DIRECT:
+      case AP_MODE_RATE_DIRECT:
+      case AP_MODE_ATTITUDE_DIRECT:
+      case AP_MODE_HOVER_DIRECT:
+      case AP_MODE_CARE_FREE_DIRECT:
+        guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT);
+        break;
+      case AP_MODE_RATE_RC_CLIMB:
+      case AP_MODE_ATTITUDE_RC_CLIMB:
+        guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB);
+        break;
+      case AP_MODE_ATTITUDE_CLIMB:
+      case AP_MODE_HOVER_CLIMB:
+        guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
+        break;
+      case AP_MODE_RATE_Z_HOLD:
+      case AP_MODE_ATTITUDE_Z_HOLD:
+      case AP_MODE_HOVER_Z_HOLD:
+        guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER);
+        break;
+      case AP_MODE_NAV:
+        guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
+        break;
+      default:
+        break;
     }
     autopilot_mode = new_autopilot_mode;
   }

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_DIRECT  14
 
 extern uint8_t autopilot_mode;
 extern uint8_t autopilot_mode_auto2;