Copter: Of_Loiter uses sensor velocity instead of integrated position

ArduCopter/control_ofloiter.pde

@@ -9,7 +9,7 @@
 // ofloiter_init - initialise ofloiter controller
 static bool ofloiter_init(bool ignore_checks)
 {
-    if (g.optflow_enabled || ignore_checks) {
+    if (optflow.enabled() || ignore_checks) {
 
         // initialize vertical speed and acceleration
         pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
@@ -18,6 +18,11 @@ static bool ofloiter_init(bool ignore_checks)
         // initialise altitude target to stopping point
         pos_control.set_target_to_stopping_point_z();
 
+        // initialise of_roll, pitch to current attitude
+        of_roll = ahrs.roll_sensor;
+        of_pitch = ahrs.pitch_sensor;
+        reset_optflow_I();
+
         return true;
     }else{
         return false;
@@ -29,6 +34,7 @@ static bool ofloiter_init(bool ignore_checks)
 static void ofloiter_run()
 {
     int16_t target_roll, target_pitch;
+    float final_roll, final_pitch;
     float target_yaw_rate = 0;
     float target_climb_rate = 0;
 
@@ -38,6 +44,8 @@ static void ofloiter_run()
         attitude_control.set_yaw_target_to_current_heading();
         attitude_control.set_throttle_out(0, false);
         pos_control.set_alt_target_to_current_alt();
+        of_roll = ahrs.roll_sensor;
+        of_pitch = ahrs.pitch_sensor;
         reset_optflow_I();
         return;
     }
@@ -72,14 +80,15 @@ static void ofloiter_run()
         // move throttle to between minimum and non-takeoff-throttle to keep us on the ground
         attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false);
         pos_control.set_alt_target_to_current_alt();
+        of_roll = ahrs.roll_sensor;
+        of_pitch = ahrs.pitch_sensor;
         reset_optflow_I();
     }else{
         // mix in user control with optical flow
-        target_roll = get_of_roll(target_roll);
-        target_pitch = get_of_pitch(target_pitch);
+        get_of_roll_pitch(target_roll, target_pitch, final_roll, final_pitch);
 
         // call attitude controller
-        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
+        attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(final_roll, final_pitch, target_yaw_rate, get_smoothing_gain());
 
         // run altitude controller
         if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
@@ -93,81 +102,50 @@ static void ofloiter_run()
     }
 }
 
-
 // calculate modified roll/pitch depending upon optical flow calculated position
-static int32_t get_of_roll(int32_t input_roll)
+static void get_of_roll_pitch(int16_t input_roll, int16_t input_pitch, float &roll_out, float &pitch_out)
 {
-    static float tot_x_cm = 0;      // total distance from target
-    static uint32_t last_of_roll_update = 0;
-    int32_t new_roll = 0;
-    int32_t p,i,d;
-
-    // check if new optflow data available
-    if( optflow.last_update != last_of_roll_update) {
-        last_of_roll_update = optflow.last_update;
-
-        // add new distance moved
-        tot_x_cm += optflow.x_cm;
-
-        // only stop roll if caller isn't modifying roll
-        if( input_roll == 0 && current_loc.alt < 1500) {
-            p = g.pid_optflow_roll.get_p(-tot_x_cm);
-            i = g.pid_optflow_roll.get_i(-tot_x_cm,1.0f);              // we could use the last update time to calculate the time change
-            d = g.pid_optflow_roll.get_d(-tot_x_cm,1.0f);
-            new_roll = p+i+d;
-        }else{
-            g.pid_optflow_roll.reset_I();
-            tot_x_cm = 0;
-            p = 0;              // for logging
-            i = 0;
-            d = 0;
-        }
-        // limit amount of change and maximum angle
-        of_roll = constrain_int32(new_roll, (of_roll-20), (of_roll+20));
-    }
-
-    // limit max angle
-    of_roll = constrain_int32(of_roll, -1000, 1000);
+    static uint32_t last_of_update = 0;
+    float dt;
 
-    return input_roll+of_roll;
-}
-
-static int32_t get_of_pitch(int32_t input_pitch)
-{
-    static float tot_y_cm = 0;  // total distance from target
-    static uint32_t last_of_pitch_update = 0;
-    int32_t new_pitch = 0;
-    int32_t p,i,d;
+    // To-Do: pass input_roll, input_pitch through to roll_out, pitch_out if input is non-zero or previous iteration was non-zero
 
     // check if new optflow data available
-    if( optflow.last_update != last_of_pitch_update ) {
-        last_of_pitch_update = optflow.last_update;
-
-        // add new distance moved
-        tot_y_cm += optflow.y_cm;
-
-        // only stop roll if caller isn't modifying pitch
-        if( input_pitch == 0 && current_loc.alt < 1500 ) {
-            p = g.pid_optflow_pitch.get_p(tot_y_cm);
-            i = g.pid_optflow_pitch.get_i(tot_y_cm, 1.0f);              // we could use the last update time to calculate the time change
-            d = g.pid_optflow_pitch.get_d(tot_y_cm, 1.0f);
-            new_pitch = p + i + d;
-        }else{
-            tot_y_cm = 0;
+    if (optflow.last_update() != last_of_update) {
+
+        // calculate dt and sanity check
+        dt = (optflow.last_update() - last_of_update) / 1000.0f;
+        if (dt > 0.2) {
+            dt = 0;
+            g.pid_optflow_roll.reset_I();
             g.pid_optflow_pitch.reset_I();
-            p = 0;              // for logging
-            i = 0;
-            d = 0;
         }
+        last_of_update = optflow.last_update();
+
+        // get latest velocity from sensor
+        const Vector2f &vel = optflow.velocity();
+
+        // allow pilot override of roll
+        if (input_roll == 0 && current_loc.alt < 1500) {
+            roll_out = g.pid_optflow_roll.get_pid(-vel.x, dt);
+            // limit amount of change and maximum angle
+            roll_out = constrain_float(roll_out, (of_roll-20), (of_roll+20));
+        } else {
+            roll_out = input_roll;
+        }
+        of_roll = roll_out;
 
-        // limit amount of change
-        of_pitch = constrain_int32(new_pitch, (of_pitch-20), (of_pitch+20));
+        if (input_pitch == 0 && current_loc.alt < 1500) {
+            pitch_out = g.pid_optflow_pitch.get_pid(vel.y, dt);
+            pitch_out = constrain_float(pitch_out, (of_pitch-20), (of_pitch+20));
+        } else {
+            pitch_out = input_pitch;
+        }
+        of_pitch = pitch_out;
+    } else {
+        roll_out = of_roll;
+        pitch_out = of_pitch;
     }
-
-    // limit max angle
-    of_pitch = constrain_int32(of_pitch, -1000, 1000);
-
-    return input_pitch+of_pitch;
 }
 
 // reset_optflow_I - reset optflow position hold I terms
@@ -175,8 +153,6 @@ static void reset_optflow_I(void)
 {
     g.pid_optflow_roll.reset_I();
     g.pid_optflow_pitch.reset_I();
-    of_roll = 0;
-    of_pitch = 0;
 }
 
 #endif // OPTFLOW == ENABLED