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

AR_AttitudeControl: add get_turn_rate_from_lat_accel

This commit is contained in:
Randy Mackay 2019-05-01 09:30:00 +09:00
parent d76894d883
commit 8907b727f6
2 changed files with 19 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -292,17 +292,7 @@ float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool m
return 0.0f; return 0.0f;
} }
// enforce minimum speed to stop oscillations when first starting to move const float desired_rate = get_turn_rate_from_lat_accel(desired_accel, speed);
if (fabsf(speed) < AR_ATTCONTROL_STEER_SPEED_MIN) {
if (is_negative(speed)) {
speed = -AR_ATTCONTROL_STEER_SPEED_MIN;
} else {
speed = AR_ATTCONTROL_STEER_SPEED_MIN;
}
}
// Calculate the desired steering rate given desired_accel and speed
const float desired_rate = desired_accel / speed;
return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right, dt); return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right, dt);
} }
@ -417,6 +407,21 @@ bool AR_AttitudeControl::get_lat_accel(float &lat_accel) const
return true; return true;
} }
// calculate the turn rate in rad/sec given a lateral acceleration (in m/s/s) and speed (in m/s)
float AR_AttitudeControl::get_turn_rate_from_lat_accel(float lat_accel, float speed) const
{
// enforce minimum speed to stop oscillations when first starting to move
if (fabsf(speed) < AR_ATTCONTROL_STEER_SPEED_MIN) {
if (is_negative(speed)) {
speed = -AR_ATTCONTROL_STEER_SPEED_MIN;
} else {
speed = AR_ATTCONTROL_STEER_SPEED_MIN;
}
}
return lat_accel / speed;
}
// return a throttle output from -1 to +1 given a desired speed in m/s (use negative speeds to travel backwards) // return a throttle output from -1 to +1 given a desired speed in m/s (use negative speeds to travel backwards)
// motor_limit should be true if motors have hit their upper or lower limits // motor_limit should be true if motors have hit their upper or lower limits
// cruise speed should be in m/s, cruise throttle should be a number from -1 to +1 // cruise speed should be in m/s, cruise throttle should be a number from -1 to +1

3
libraries/APM_Control/AR_AttitudeControl.h
View File

@ -79,6 +79,9 @@ public:
// get actual lateral acceleration in m/s/s. returns true on success. For reporting purposes only // get actual lateral acceleration in m/s/s. returns true on success. For reporting purposes only
bool get_lat_accel(float &lat_accel) const; bool get_lat_accel(float &lat_accel) const;
// calculate the turn rate in rad/sec given a lateral acceleration (in m/s/s) and speed (in m/s)
float get_turn_rate_from_lat_accel(float lat_accel, float speed) const;
// //
// throttle / speed controller // throttle / speed controller
// //