Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AR_AttitudeControl: support vectored thrust 

steering output is not scaled for speed
steering integrator does not build up at low speed
This commit is contained in:
Randy Mackay 2018-04-09 21:11:54 +09:00
parent 38b79cf0bc
commit 3f1f68b130
2 changed files with 11 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -173,7 +173,7 @@ AR_AttitudeControl::AR_AttitudeControl(AP_AHRS &ahrs) :
// return a steering servo output from -1.0 to +1.0 given a desired lateral acceleration rate in m/s/s.
// positive lateral acceleration is to the right.
float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed)
float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool skid_steering, bool vect_thrust, bool motor_limit_left, bool motor_limit_right, bool reversed)
{
// record desired accel for reporting purposes
_steer_lat_accel_last_ms = AP_HAL::millis();
@ -203,11 +203,11 @@ float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool s
desired_rate *= -1.0f;
}
return get_steering_out_rate(desired_rate, skid_steering, motor_limit_left, motor_limit_right, reversed);
return get_steering_out_rate(desired_rate, skid_steering, vect_thrust, motor_limit_left, motor_limit_right, 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)
float AR_AttitudeControl::get_steering_out_heading(float heading_rad, bool skid_steering, bool vect_thrust, 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);
@ -215,12 +215,12 @@ float AR_AttitudeControl::get_steering_out_heading(float heading_rad, bool skid_
// Calculate the desired turn rate (in radians) from the angle error (also in radians)
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);
return get_steering_out_rate(desired_rate, skid_steering, vect_thrust, motor_limit_left, motor_limit_right, reversed);
}
// return a steering servo output from -1 to +1 given a
// desired yaw rate in radians/sec. Positive yaw is to the right.
float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed)
float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool skid_steering, bool vect_thrust, bool motor_limit_left, bool motor_limit_right, bool reversed)
{
// calculate dt
const uint32_t now = AP_HAL::millis();
@ -250,8 +250,9 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool skid_st
float scaler = 1.0f;
bool low_speed = false;
// scaler to linearize output because turn rate increases as vehicle speed increases on non-skid steering vehicles
if (!skid_steering) {
// scaler to linearize output because turn rate increases as vehicle speed increases
// on non-skid steering and non-vectored thrust vehicles
if (!skid_steering && !vect_thrust) {
// get speed forward
float speed;

6
libraries/APM_Control/AR_AttitudeControl.h
View File

@ -45,14 +45,14 @@ public:
// return a steering servo output from -1.0 to +1.0 given a desired lateral acceleration rate in m/s/s.
// 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);
float get_steering_out_lat_accel(float desired_accel, bool skid_steering, bool vect_thrust, 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);
float get_steering_out_heading(float heading_rad, bool skid_steering, bool vect_thrust, 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.
float get_steering_out_rate(float desired_rate, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed);
float get_steering_out_rate(float desired_rate, bool skid_steering, bool vect_thrust, bool motor_limit_left, bool motor_limit_right, bool reversed);
// get latest desired turn rate in rad/sec recorded during calls to get_steering_out_rate. For reporting purposes only
float get_desired_turn_rate() const;