[opticflow] controller

conf/messages.xml

@@ -2019,7 +2019,7 @@
     <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="KILL|FAILSAFE|HOME|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|FORWARD"/>
+    <field name="ap_mode"      type="uint8" values="KILL|FAILSAFE|HOME|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|FORWARD|MODULE"/>
     <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|CF"/>

conf/modules/cv_opticflow.xml

@@ -37,6 +37,7 @@
   <makefile target="ap">
     <define name="ARDRONE_VIDEO_PORT" value="2002" />
     <define name="USE_ARDRONE_VIDEO" />
+    <define name="USE_MODULE_OUTERLOOP" value="1" />
 
     <file name="opticflow_module.c"/>
     <file name="opticflow_code.c" dir="modules/computer_vision/opticflow"/>

sw/airborne/firmwares/rotorcraft/autopilot.c

@@ -423,6 +423,9 @@ void autopilot_set_mode(uint8_t new_autopilot_mode)
       case AP_MODE_NAV:
         guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
         break;
+      case AP_MODE_MODULE_OUTERLOOP:
+        guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_OUTERLOOP);
+        break;
       default:
         break;
     }
@@ -464,6 +467,9 @@ void autopilot_set_mode(uint8_t new_autopilot_mode)
       case AP_MODE_NAV:
         guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
         break;
+      case AP_MODE_MODULE_OUTERLOOP:
+        guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE_OUTERLOOP);
+        break;
       default:
         break;
     }

sw/airborne/firmwares/rotorcraft/autopilot.h

@@ -50,6 +50,7 @@
 #define AP_MODE_RC_DIRECT         14  // Safety Pilot Direct Commands for helicopter direct control
 #define AP_MODE_CARE_FREE_DIRECT  15
 #define AP_MODE_FORWARD           16
+#define AP_MODE_MODULE_OUTERLOOP  17
 
 extern uint8_t autopilot_mode;
 extern uint8_t autopilot_mode_auto2;

sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c

@@ -252,6 +252,7 @@ void guidance_h_mode_changed(uint8_t new_mode)
         stabilization_attitude_enter();
       break;
 
+    case GUIDANCE_H_MODE_MODULE_OUTERLOOP:
     case GUIDANCE_H_MODE_NAV:
       guidance_h_nav_enter();
 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
@@ -304,6 +305,7 @@ void guidance_h_read_rc(bool_t  in_flight)
 #endif
       break;
 
+    case GUIDANCE_H_MODE_MODULE_OUTERLOOP:
     case GUIDANCE_H_MODE_NAV:
       if (radio_control.status == RC_OK) {
         stabilization_attitude_read_rc_setpoint_eulers(&guidance_h_rc_sp, in_flight, FALSE, FALSE);
@@ -386,6 +388,15 @@ void guidance_h_run(bool_t  in_flight)
       }
       stabilization_attitude_run(in_flight);
       break;
+    case GUIDANCE_H_MODE_MODULE_OUTERLOOP:
+      if (!in_flight) {
+        guidance_h_nav_enter();
+#if USE_MODULE_OUTERLOOP==1
+        guidance_module_run(in_flight);
+#endif
+      }
+
+      break;
 
     default:
       break;

sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h

@@ -49,14 +49,15 @@
 #define GUIDANCE_H_USE_SPEED_REF TRUE
 #endif
 
-#define GUIDANCE_H_MODE_KILL        0
+#define GUIDANCE_H_MODE_KILL             0
-#define GUIDANCE_H_MODE_RATE        1
+#define GUIDANCE_H_MODE_RATE             1
-#define GUIDANCE_H_MODE_ATTITUDE    2
+#define GUIDANCE_H_MODE_ATTITUDE         2
-#define GUIDANCE_H_MODE_HOVER       3
+#define GUIDANCE_H_MODE_HOVER            3
-#define GUIDANCE_H_MODE_NAV         4
+#define GUIDANCE_H_MODE_NAV              4
-#define GUIDANCE_H_MODE_RC_DIRECT   5
+#define GUIDANCE_H_MODE_RC_DIRECT        5
-#define GUIDANCE_H_MODE_CARE_FREE   6
+#define GUIDANCE_H_MODE_CARE_FREE        6
-#define GUIDANCE_H_MODE_FORWARD     7
+#define GUIDANCE_H_MODE_FORWARD          7
+#define GUIDANCE_H_MODE_MODULE_OUTERLOOP 8
 
 
 extern uint8_t guidance_h_mode;

sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c

@@ -232,6 +232,7 @@ void guidance_v_mode_changed(uint8_t new_mode)
     case GUIDANCE_V_MODE_RC_CLIMB:
     case GUIDANCE_V_MODE_CLIMB:
       guidance_v_zd_sp = 0;
+    case GUIDANCE_V_MODE_MODULE_OUTERLOOP:
     case GUIDANCE_V_MODE_NAV:
       guidance_v_z_sum_err = 0;
       GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
@@ -307,6 +308,19 @@ void guidance_v_run(bool_t in_flight)
         stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
       break;
 
+    case GUIDANCE_V_MODE_MODULE_OUTERLOOP:
+      guidance_v_z_sp = -nav_flight_altitude;
+      guidance_v_zd_sp = 0;
+      gv_update_ref_from_z_sp(guidance_v_z_sp);
+      run_hover_loop(in_flight);
+#if !NO_RC_THRUST_LIMIT
+      /* use rc limitation if available */
+      if (radio_control.status == RC_OK) {
+        stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t);
+      } else
+#endif
+        stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
+      break;
     case GUIDANCE_V_MODE_NAV: {
       if (vertical_mode == VERTICAL_MODE_ALT) {
         guidance_v_z_sp = -nav_flight_altitude;

sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h

@@ -32,12 +32,13 @@
 #include "firmwares/rotorcraft/guidance/guidance_v_ref.h"
 #include "firmwares/rotorcraft/guidance/guidance_v_adapt.h"
 
-#define GUIDANCE_V_MODE_KILL      0
+#define GUIDANCE_V_MODE_KILL             0
-#define GUIDANCE_V_MODE_RC_DIRECT 1
+#define GUIDANCE_V_MODE_RC_DIRECT        1
-#define GUIDANCE_V_MODE_RC_CLIMB  2
+#define GUIDANCE_V_MODE_RC_CLIMB         2
-#define GUIDANCE_V_MODE_CLIMB     3
+#define GUIDANCE_V_MODE_CLIMB            3
-#define GUIDANCE_V_MODE_HOVER     4
+#define GUIDANCE_V_MODE_HOVER            4
-#define GUIDANCE_V_MODE_NAV       5
+#define GUIDANCE_V_MODE_NAV              5
+#define GUIDANCE_V_MODE_MODULE_OUTERLOOP 6
 
 extern uint8_t guidance_v_mode;
 

sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c

@@ -69,9 +69,6 @@ float Error_Vely;
 unsigned char saturateX = 0, saturateY = 0;
 unsigned int set_heading;
 
-// TODO FIX
-#define AP_MODE_VISION_HOVER 3
-
 
 void init_hover_stabilization_onvision()
 {
@@ -95,7 +92,7 @@ void init_hover_stabilization_onvision()
 
 void run_hover_stabilization_onvision(void)
 {
-  if (autopilot_mode == AP_MODE_VISION_HOVER) {
+  if (autopilot_mode == AP_MODE_MODULE_OUTERLOOP) {
     run_opticflow_hover();
   } else {
     Velx_Int = 0;

sw/airborne/modules/computer_vision/opticflow_module.c

@@ -97,6 +97,12 @@ void opticflow_module_run(void)
   }
 }
 
+void guidance_module_run(bool_t inflight)
+{
+  nav_flight_altitude = -1;
+}
+
+
 /////////////////////////////////////////////////////////////////////////
 // COMPUTER VISION THREAD
 

sw/airborne/modules/computer_vision/opticflow_module.h

@@ -29,12 +29,16 @@
 #ifndef OPTICFLOW_LAND_H
 #define OPTICFLOW_LAND_H
 
+#include "std.h"
+
 // Module functions
 extern void opticflow_module_init(void);
 extern void opticflow_module_run(void);
 extern void opticflow_module_start(void);
 extern void opticflow_module_stop(void);
 
+extern void guidance_module_run(bool_t inflight);
+
 // Frame Rate
 extern float FPS;
 struct timeval;