From cbe56bba257d9625cda48cd851c6208f0f553d90 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 6 Dec 2013 15:08:11 +0900 Subject: [PATCH] Copter: integrate AC_AttitudeControl --- ArduCopter/ArduCopter.pde | 86 +++++++++++++----- ArduCopter/Attitude.pde | 9 ++ ArduCopter/control_stabilize.pde | 146 +++++++++++++++++++++++++++++++ 3 files changed, 220 insertions(+), 21 deletions(-) create mode 100644 ArduCopter/control_stabilize.pde diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 421a18969a..e1f1dd41bc 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -108,6 +108,7 @@ #include #include // PI library #include // PID library +#include // Attitude control library #include // RC Channel Library #include // AP Motors library #include // 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(); diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 65478f7bba..3dbdcb1b89 100644 --- a/ArduCopter/Attitude.pde +++ b/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) { diff --git a/ArduCopter/control_stabilize.pde b/ArduCopter/control_stabilize.pde new file mode 100644 index 0000000000..062ad3be5e --- /dev/null +++ b/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() +{ +} \ No newline at end of file