Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: bug fix for stabilize_run's yaw control

Browse Source
This commit is contained in:
Randy Mackay 2013-12-16 17:27:43 +09:00 committed by Andrew Tridgell
parent 1fdfa751e6
commit 0431b7e1d4
1 changed files with 16 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduCopter/control_stabilize.pde
View File

@ -36,8 +36,8 @@ static void acro_run()
// should be called at 100hz or more // should be called at 100hz or more
static void stabilize_run() static void stabilize_run()
{ {
Vector3f angle_target; // for roll and pitch angle targets Vector3f angle_target = attitude_control.angle_ef_targets(); // for roll, pitch and yaw angular targets
Vector3f rate_stab_ef_target; // for yaw rate target Vector3f rate_stab_ef_target; // for yaw rate target. Note Vector3f initialises all values to zero in constructor
int16_t target_roll, target_pitch; int16_t target_roll, target_pitch;
// apply SIMPLE mode transform to pilot inputs // apply SIMPLE mode transform to pilot inputs
@ -49,6 +49,11 @@ static void stabilize_run()
angle_target.x = target_roll; angle_target.x = target_roll;
angle_target.y = target_pitch; angle_target.y = target_pitch;
// set target heading to current heading while landed
if (ap.land_complete) {
angle_target.z = ahrs.yaw_sensor;
}
// set earth-frame angular targets // set earth-frame angular targets
attitude_control.angle_ef_targets(angle_target); attitude_control.angle_ef_targets(angle_target);
@ -57,7 +62,9 @@ static void stabilize_run()
attitude_control.angle_to_rate_ef_pitch(); attitude_control.angle_to_rate_ef_pitch();
// get pilot's desired yaw rate // get pilot's desired yaw rate
if (!failsafe.radio && !ap.land_complete) {
rate_stab_ef_target.z = get_pilot_desired_yaw_rate(g.rc_4.control_in); 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 // 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 // To-Do: this is quite wasteful to update the entire target vector when only yaw is used
@ -70,6 +77,12 @@ static void stabilize_run()
// convert earth-frame rates to body-frame rates // convert earth-frame rates to body-frame rates
attitude_control.rate_ef_targets_to_bf(); attitude_control.rate_ef_targets_to_bf();
// refetch angle targets for reporting
angle_target = attitude_control.angle_ef_targets();
control_roll = angle_target.x;
control_pitch = angle_target.y;
control_yaw = angle_target.z;
// body-frame rate controller is run directly from 100hz loop // body-frame rate controller is run directly from 100hz loop
// To-Do: add throttle control for stabilize mode here? // To-Do: add throttle control for stabilize mode here?