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

AR_AttitudeControl: replace get_steering_out_angle_error with heading

Browse Source 
internally uses AHRS's heading to determine angle error so overally a non-functional change
This commit is contained in:
Randy Mackay 2018-03-16 18:39:52 +09:00
parent 8186ce0d32
commit a8857726d1
2 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -206,11 +206,14 @@ float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool s
return get_steering_out_rate(desired_rate, skid_steering, motor_limit_left, motor_limit_right, reversed);
}
// return a steering servo output from -1 to +1 given a yaw error in radians
float AR_AttitudeControl::get_steering_out_angle_error(float angle_err, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed)
// return a steering servo output from -1 to +1 given a heading in radians
float AR_AttitudeControl::get_steering_out_heading(float heading_rad, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed)
{
// calculate heading error (in radians)
const float yaw_error = wrap_PI(heading_rad - _ahrs.yaw);
// Calculate the desired turn rate (in radians) from the angle error (also in radians)
const float desired_rate = _steer_angle_p.get_p(angle_err);
const float desired_rate = _steer_angle_p.get_p(yaw_error);
return get_steering_out_rate(desired_rate, skid_steering, motor_limit_left, motor_limit_right, reversed);
}

4
libraries/APM_Control/AR_AttitudeControl.h
View File

@ -47,8 +47,8 @@ public:
// positive lateral acceleration is to the right.
float get_steering_out_lat_accel(float desired_accel, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed);
// return a steering servo output from -1 to +1 given an angle error in radians
float get_steering_out_angle_error(float angle_err, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed);
// return a steering servo output from -1 to +1 given a heading in radians
float get_steering_out_heading(float heading_rad, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed);
// return a steering servo output from -1 to +1 given a
// desired yaw rate in radians/sec. Positive yaw is to the right.