Sub: Implement NAV_CMD_LOITER_TURNS

ArduSub/Sub.h

@@ -838,6 +838,7 @@ private:
     void auto_terrain_recover_run(void);
 
     void translate_wpnav_rp(float &lateral_out, float &forward_out);
+    void translate_circle_nav_rp(float &lateral_out, float &forward_out);
 
     bool surface_init(bool ignore_flags);
     void surface_run();

ArduSub/control_auto.cpp

@@ -371,11 +371,18 @@ void Sub::auto_circle_run()
     // call circle controller
     circle_nav.update();
 
+    float lateral_out, forward_out;
+    translate_circle_nav_rp(lateral_out, forward_out);
+
+    // Send to forward/lateral outputs
+    motors.set_lateral(lateral_out);
+    motors.set_forward(forward_out);
+
     // call z-axis position controller
     pos_control.update_z_controller();
 
     // roll & pitch from waypoint controller, yaw rate from pilot
-    attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true, get_smoothing_gain());
+    attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true, get_smoothing_gain());
 }
 
 #if NAV_GUIDED == ENABLED

ArduSub/motors.cpp

@@ -220,3 +220,20 @@ void Sub::translate_wpnav_rp(float &lateral_out, float &forward_out)
     lateral_out = (float)lateral/(float)aparm.angle_max;
     forward_out = (float)forward/(float)aparm.angle_max;
 }
+
+// translate wpnav roll/pitch outputs to lateral/forward
+void Sub::translate_circle_nav_rp(float &lateral_out, float &forward_out)
+{
+    // get roll and pitch targets in centidegrees
+    int32_t lateral = circle_nav.get_roll();
+    int32_t forward = -circle_nav.get_pitch(); // output is reversed
+
+    // constrain target forward/lateral values
+    // The outputs of wp_nav.get_roll and get_pitch should already be constrained to these values
+    lateral = constrain_int16(lateral, -aparm.angle_max, aparm.angle_max);
+    forward = constrain_int16(forward, -aparm.angle_max, aparm.angle_max);
+
+    // Normalize
+    lateral_out = (float)lateral/(float)aparm.angle_max;
+    forward_out = (float)forward/(float)aparm.angle_max;
+}