Copter: integrate AC_AttitudeControl

ArduCopter/ArduCopter.pde

@@ -108,6 +108,7 @@
 #include <AP_AHRS.h>
 #include <APM_PI.h>             // PI library
 #include <AC_PID.h>             // PID library
+#include <AC_AttitudeControl.h>  // Attitude control library
 #include <RC_Channel.h>         // RC Channel Library
 #include <AP_Motors.h>          // AP Motors library
 #include <AP_RangeFinder.h>     // Range finder library
@@ -486,6 +487,13 @@ static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.single_servo_1,
 static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4);
 #endif
 
+////////////////////////////////////////////////////////////////////////////////
+// Attitude controller
+////////////////////////////////////////////////////////////////////////////////
+AC_AttitudeControl attitude_control(ahrs, ins, aparm, motors, g.pi_stabilize_roll, g.pi_stabilize_pitch, g.pi_stabilize_yaw,
+                        g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw,
+                        g.rc_1.servo_out, g.rc_2.servo_out, g.rc_4.servo_out, g.rc_3.servo_out);
+
 ////////////////////////////////////////////////////////////////////////////////
 // PIDs
 ////////////////////////////////////////////////////////////////////////////////
@@ -981,7 +989,7 @@ static void fast_loop()
     }
 
     // run low level rate controllers that only require IMU data
-    run_rate_controllers();
+    attitude_control.rate_controller_run();
 
     // write out the servo PWM values
     // ------------------------------
@@ -1004,13 +1012,8 @@ static void fast_loop()
     read_radio();
     read_control_switch();
 
-    // custom code/exceptions for flight modes
-    // ---------------------------------------
-    update_yaw_mode();
-    update_roll_pitch_mode();
-
-    // update targets to rate controllers
-    update_rate_contoller_targets();
+    // run the attitude controllers
+    update_flight_mode();
 }
 
 // throttle_loop - should be run at 50 hz
@@ -1587,6 +1590,60 @@ void exit_roll_pitch_mode(uint8_t old_roll_pitch_mode)
 #endif
 }
 
+// update_flight_mode - calls the appropriate attitude controllers based on flight mode
+// called at 100hz or more
+static void update_flight_mode()
+{
+    switch (control_mode) {
+        case ACRO:
+            acro_run();
+            break;
+
+        case STABILIZE:
+            stabilize_run();
+            break;
+
+        case ALT_HOLD:
+            althold_run();
+            break;
+
+        case AUTO:
+            auto_run();
+            break;
+
+        case CIRCLE:
+            circle_run();
+            break;
+
+        case LOITER:
+            loiter_run();
+            break;
+
+        case GUIDED:
+            guided_run();
+            break;
+
+        case LAND:
+            land_run();
+            break;
+
+        case RTL:
+            rtl_run();
+            break;
+
+        case OF_LOITER:
+            ofloiter_run();
+            break;
+
+        case DRIFT:
+            drift_run();
+            break;
+
+        case SPORT:
+            sport_run();
+            break;
+    }
+}
 
 // update_roll_pitch_mode - run high level roll and pitch controllers
 // 100hz update rate
@@ -1617,19 +1674,6 @@ void update_roll_pitch_mode(void)
 #endif  // HELI_FRAME
         break;
 
-    case ROLL_PITCH_STABLE:
-        // apply SIMPLE mode transform
-        update_simple_mode();
-
-        // convert pilot input to lean angles
-        get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
-
-        // pass desired roll, pitch to stabilize attitude controllers
-        get_stabilize_roll(control_roll);
-        get_stabilize_pitch(control_pitch);
-
-        break;
-
     case ROLL_PITCH_AUTO:
         // copy latest output from nav controller to stabilize controller
         control_roll = wp_nav.get_desired_roll();

ArduCopter/Attitude.pde

@@ -60,6 +60,15 @@ static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int
     pitch_out = (int16_t)(pitch_in_filtered * _scaler);
 }
 
+// get_pilot_desired_heading - transform pilot's yaw input into a desired heading
+// returns desired angle in centi-degrees
+// To-Do: return heading as a float?
+static float get_pilot_desired_yaw_rate(int16_t stick_angle)
+{
+    // convert pilot input to the desired yaw rate
+    return stick_angle * g.acro_yaw_p;
+}
+
 static void
 get_stabilize_roll(int32_t target_angle)
 {

ArduCopter/control_stabilize.pde

@@ -0,0 +1,146 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// acro_run - runs the acro controller
+// should be called at 100hz or more
+static void acro_run()
+{
+}
+
+// stabilize_run - runs the main stabilize controller
+// should be called at 100hz or more
+static void stabilize_run()
+{
+    Vector3f angle_target;          // for roll and pitch angle targets
+    Vector3f rate_stab_ef_target;    // for yaw rate target
+    int16_t target_roll, target_pitch;
+
+    // debug -- remove me!
+    cliSerial->printf_P(PSTR("\nstabilize_run!"));
+
+    // apply SIMPLE mode transform to pilot inputs
+    update_simple_mode();
+
+    // convert pilot input to lean angles
+    // To-Do: convert get_pilot_desired_lean_angles to return angles as floats
+    get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, target_roll, target_pitch);
+    angle_target.x = target_roll;
+    angle_target.y = target_pitch;
+
+    // set earth-frame angular targets
+    attitude_control.angle_ef_targets(angle_target);
+
+    // convert earth-frame angle targets to earth-frame rate targets
+    attitude_control.angle_to_rate_ef_roll();
+    attitude_control.angle_to_rate_ef_pitch();
+
+    // get pilot's desired yaw rate
+    rate_stab_ef_target.z = get_pilot_desired_yaw_rate(g.rc_4.control_in);
+
+    // set earth-frame rate stabilize target for yaw with pilot's desired yaw
+    // To-Do: this is quite wasteful to update the entire target vector when only yaw is used
+    attitude_control.rate_stab_ef_targets(rate_stab_ef_target);
+
+    // convert earth-frame stabilize rate to regular rate target
+    // To-Do: replace G_Dt below
+    attitude_control.rate_stab_ef_to_rate_ef_yaw(G_Dt);
+
+    // convert earth-frame rates to body-frame rates
+    attitude_control.rate_ef_targets_to_bf();
+
+    // body-frame rate controller is run directly from 100hz loop
+
+    // To-Do: add throttle control for stabilize mode here?
+}
+
+// althold_run - runs the althold controller
+// should be called at 100hz or more
+static void althold_run()
+{
+}
+
+// auto_run - runs the auto controller
+// should be called at 100hz or more
+static void auto_run()
+{
+    Vector3f angle_target;
+
+    // user input, although ignored is put into control_roll and pitch for reporting purposes
+    control_roll = g.rc_1.control_in;
+    control_pitch = g.rc_2.control_in;
+
+    // copy latest output from nav controller to stabilize controller
+    angle_target.x = wp_nav.get_desired_roll();
+    angle_target.y = wp_nav.get_desired_pitch();
+
+    // To-Do: handle yaw
+    angle_target.z = control_yaw;
+
+    // copy angle targets for reporting purposes
+    control_roll = angle_target.x;
+    control_pitch = angle_target.y;
+
+    // To-Do: shorten below by moving these often used steps into a single function in the AC_AttitudeControl lib
+
+    // set earth-frame angular targets
+    attitude_control.angle_ef_targets(angle_target);
+
+    // convert earth-frame angle targets to earth-frame rate targets
+    attitude_control.angle_to_rate_ef_roll();
+    attitude_control.angle_to_rate_ef_pitch();
+    attitude_control.angle_to_rate_ef_yaw();
+
+    // convert earth-frame rates to body-frame rates
+    attitude_control.rate_ef_targets_to_bf();
+
+    // body-frame rate controller is run directly from 100hz loop
+}
+
+// circle_run - runs the circle controller
+// should be called at 100hz or more
+static void circle_run()
+{
+}
+
+// loiter_run - runs the loiter controller
+// should be called at 100hz or more
+static void loiter_run()
+{
+}
+
+// guided_run - runs the guided controller
+// should be called at 100hz or more
+static void guided_run()
+{
+}
+
+// land_run - runs the land controller
+// should be called at 100hz or more
+static void land_run()
+{
+    verify_land();
+}
+
+// rtl_run - runs the return-to-launch controller
+// should be called at 100hz or more
+static void rtl_run()
+{
+    verify_RTL();
+}
+
+// ofloiter_run - runs the optical flow loiter controller
+// should be called at 100hz or more
+static void ofloiter_run()
+{
+}
+
+// drift_run - runs the drift controller
+// should be called at 100hz or more
+static void drift_run()
+{
+}
+
+// sport_run - runs the sport controller
+// should be called at 100hz or more
+static void sport_run()
+{
+}