Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-06 16:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_AttitudeControl: support vectored thrust

Browse Source 
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 32ef6e85eb
commit e2ba59ab8f
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. // 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. // 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 // record desired accel for reporting purposes
_steer_lat_accel_last_ms = AP_HAL::millis(); _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; 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 // 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) // calculate heading error (in radians)
const float yaw_error = wrap_PI(heading_rad - _ahrs.yaw); 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) // 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); 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 // return a steering servo output from -1 to +1 given a
// desired yaw rate in radians/sec. Positive yaw is to the right. // 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 // calculate dt
const uint32_t now = AP_HAL::millis(); 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; float scaler = 1.0f;
bool low_speed = false; bool low_speed = false;
// scaler to linearize output because turn rate increases as vehicle speed increases on non-skid steering vehicles // scaler to linearize output because turn rate increases as vehicle speed increases
if (!skid_steering) { // on non-skid steering and non-vectored thrust vehicles
if (!skid_steering && !vect_thrust) {
// get speed forward // get speed forward
float speed; 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. // 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. // 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 // 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 // return a steering servo output from -1 to +1 given a
// desired yaw rate in radians/sec. Positive yaw is to the right. // 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 // 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; float get_desired_turn_rate() const;