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

Rover: use AR_AttitudeControl for throttle control

Browse Source 
also direct throttle nudge replaced with speed nudge
calc_speed_max estimates vehicle's top speed based on cruise-speed and
cruise-throttle
steering mode now provides total target speed instead of using speed plug
throttle nudge
motor limits provided to attitude control to stop i-term buildup
uses negative desired speed instead of reversed flag
reporting to GCS uses new throttle controller
braking is simply enabled and allows a reverse motor output regardless of vehicle
speed
This commit is contained in:
Randy Mackay 2017-08-09 09:24:30 +09:00
parent 61d1ced7aa
commit d99108f3bc
12 changed files with 98 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/GCS_Mavlink.cpp
View File

@ -242,7 +242,7 @@ void Rover::send_pid_tuning(mavlink_channel_t chan)
}
}
if (g.gcs_pid_mask & 2) {
pid_info = &g.pidSpeedThrottle.get_pid_info();
pid_info = &g2.attitude_control.get_throttle_speed_pid().get_pid_info();
mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ,
pid_info->desired,
0,

2
APMrover2/Log.cpp
View File

@ -155,7 +155,7 @@ void Rover::Log_Write_Attitude()
// log steering rate controller
DataFlash.Log_Write_PID(LOG_PIDS_MSG, g2.attitude_control.get_steering_rate_pid().get_pid_info());
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pidSpeedThrottle.get_pid_info());
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g2.attitude_control.get_throttle_speed_pid().get_pid_info());
}
struct PACKED log_Rangefinder {

22
APMrover2/Parameters.cpp
View File

@ -123,24 +123,6 @@ const AP_Param::Info Rover::var_info[] = {
// @User: Standard
GSCALAR(speed_turn_gain, "SPEED_TURN_GAIN", 50),
// @Param: BRAKING_PERCENT
// @DisplayName: Percentage braking to apply
// @Description: The maximum reverse throttle braking percentage to apply when cornering
// @Units: %
// @Range: 0 100
// @Increment: 1
// @User: Standard
GSCALAR(braking_percent, "BRAKING_PERCENT", 0),
// @Param: BRAKING_SPEEDERR
// @DisplayName: Speed error at which to apply braking
// @Description: The amount of overspeed error at which to start applying braking
// @Units: m/s
// @Range: 0 100
// @Increment: 1
// @User: Standard
GSCALAR(braking_speederr, "BRAKING_SPEEDERR", 3),
// @Param: PIVOT_TURN_ANGLE
// @DisplayName: Pivot turn angle
// @Description: Navigation angle threshold in degrees to switch to pivot steering when SKID_STEER_OUT is 1. This allows you to setup a skid steering rover to turn on the spot in auto mode when the angle it needs to turn it greater than this angle. An angle of zero means to disable pivot turning. Note that you will probably also want to set a low value for WP_RADIUS to get neat turns.
@ -341,10 +323,6 @@ const AP_Param::Info Rover::var_info[] = {
// @User: Standard
GSCALAR(turn_max_g, "TURN_MAX_G", 2.0f),
// @Group: SPEED2THR_
// @Path: ../libraries/PID/PID.cpp
GGROUP(pidSpeedThrottle, "SPEED2THR_", PID),
// variables not in the g class which contain EEPROM saved variables
// @Group: COMPASS_

20
APMrover2/Parameters.h
View File

@ -43,8 +43,8 @@ public:
k_param_battery_curr_pin,
// braking
k_param_braking_percent = 30,
k_param_braking_speederr,
k_param_braking_percent_old = 30, // unused
k_param_braking_speederr_old, // unused
// misc2
k_param_log_bitmask = 40,
@ -180,7 +180,7 @@ public:
// 240: PID Controllers
k_param_pidNavSteer = 230,
k_param_pidServoSteer, // unused
k_param_pidSpeedThrottle,
k_param_pidSpeedThrottle_old, // unused
// high RC channels
k_param_rc_9_old = 235,
@ -215,10 +215,6 @@ public:
AP_Int8 reset_switch_chan;
AP_Int8 initial_mode;
// braking
AP_Int8 braking_percent;
AP_Float braking_speederr;
// Telemetry control
//
AP_Int16 sysid_this_mav;
@ -276,15 +272,7 @@ public:
//
AP_Float waypoint_radius;
// PID controllers
//
PID pidSpeedThrottle;
Parameters() :
// PID controller initial P initial I initial D initial imax
//-----------------------------------------------------------------------------------
pidSpeedThrottle (0.7f, 0.2f, 0.2f, 4000)
{}
Parameters() {}
};
/*

1
APMrover2/Rover.h
View File

@ -38,7 +38,6 @@
#include <AP_Mission/AP_Mission.h> // Mission command library
#include <AP_Rally/AP_Rally.h>
#include <AP_Terrain/AP_Terrain.h>
#include <PID/PID.h> // PID library
#include <AC_PID/AC_P.h>
#include <AC_PID/AC_PID.h>
#include <RC_Channel/RC_Channel.h> // RC Channel Library

3
APMrover2/control_modes.cpp
View File

@ -76,9 +76,6 @@ void Rover::read_control_switch()
}
oldSwitchPosition = switchPosition;
// reset speed integrator
g.pidSpeedThrottle.reset_I();
}
switch_debouncer = false;

98
APMrover2/mode.cpp
View File

@ -17,7 +17,6 @@ void Mode::exit()
_exit();
lateral_acceleration = 0.0f;
rover.g.pidSpeedThrottle.reset_I();
}
bool Mode::enter()
@ -50,56 +49,75 @@ void Mode::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
_desired_speed = target_speed;
}
void Mode::calc_throttle(float target_speed, bool reversed)
void Mode::calc_throttle(float target_speed, bool nudge_allowed)
{
// get ground speed from vehicle
const float &groundspeed = rover.ground_speed;
// add in speed nudging
if (nudge_allowed) {
target_speed = calc_speed_nudge(target_speed, g.speed_cruise, g.throttle_cruise / 100.0f);
}
// calculate ground speed and ground speed error
_speed_error = fabsf(target_speed) - groundspeed;
// call throttle controller and convert output to -100 to +100 range
float throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, g2.motors.have_skid_steering(), g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise / 100.0f);
const float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise;
const float throttle_target = throttle_base + calc_throttle_nudge();
float throttle = throttle_target + (g.pidSpeedThrottle.get_pid(_speed_error * 100.0f) / 100.0f);
if (reversed) {
g2.motors.set_throttle(constrain_int16(-throttle, -g.throttle_max, -g.throttle_min));
// apply limits on throttle
if (is_negative(throttle_out)) {
g2.motors.set_throttle(constrain_float(throttle_out, -g.throttle_max, -g.throttle_min));
} else {
g2.motors.set_throttle(constrain_int16(throttle, g.throttle_min, g.throttle_max));
}
if (!reversed && g.braking_percent != 0 && _speed_error < -g.braking_speederr) {
// the user has asked to use reverse throttle to brake. Apply
// it in proportion to the ground speed error, but only when
// our ground speed error is more than BRAKING_SPEEDERR.
//
// We use a linear gain, with 0 gain at a ground speed error
// of braking_speederr, and 100% gain when groundspeed_error
// is 2*braking_speederr
const float brake_gain = constrain_float(((-_speed_error)-g.braking_speederr)/g.braking_speederr, 0.0f, 1.0f);
const int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
g2.motors.set_throttle(constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min));
g2.motors.set_throttle(constrain_float(throttle_out, g.throttle_min, g.throttle_max));
}
}
// calculate pilot input to nudge throttle up or down
int16_t Mode::calc_throttle_nudge()
// estimate maximum vehicle speed (in m/s)
float Mode::calc_speed_max(float cruise_speed, float cruise_throttle)
{
// get pilot throttle input (-100 to +100)
int16_t pilot_throttle = rover.channel_throttle->get_control_in();
int16_t throttle_nudge = 0;
float speed_max;
// Check if the throttle value is above 50% and we need to nudge
// Make sure its above 50% in the direction we are travelling
if ((fabsf(pilot_throttle) > 50.0f) &&
(((pilot_throttle < 0) && rover.in_reverse) ||
((pilot_throttle > 0) && !rover.in_reverse))) {
throttle_nudge = (rover.g.throttle_max - rover.g.throttle_cruise) * ((fabsf(rover.channel_throttle->norm_input()) - 0.5f) / 0.5f);
// sanity checks
if (cruise_throttle > 1.0f || cruise_throttle < 0.05f) {
speed_max = cruise_speed;
} else {
// project vehicle's maximum speed
speed_max = (1.0f / cruise_throttle) * cruise_speed;
}
return throttle_nudge;
// constrain to 30m/s (108km/h) and return
return constrain_float(speed_max, 0.0f, 30.0f);
}
// calculate pilot input to nudge speed up or down
// target_speed should be in meters/sec
// cruise_speed is vehicle's cruising speed, cruise_throttle is the throttle (from -1 to +1) that achieves the cruising speed
// return value is a new speed (in m/s) which up to the projected maximum speed based on the cruise speed and cruise throttle
float Mode::calc_speed_nudge(float target_speed, float cruise_speed, float cruise_throttle)
{
// return immediately if pilot is not attempting to nudge speed
// pilot can nudge up speed if throttle (in range -100 to +100) is above 50% of center in direction of travel
const int16_t pilot_throttle = constrain_int16(rover.channel_throttle->get_control_in(), -100, 100);
if ((pilot_throttle <= 50 && is_positive(target_speed)) ||
(pilot_throttle >= -50 && is_negative(target_speed))) {
return target_speed;
}
// sanity checks
if (cruise_throttle > 1.0f || cruise_throttle < 0.05f) {
return target_speed;
}
// project vehicle's maximum speed
const float vehicle_speed_max = calc_speed_max(cruise_speed, cruise_throttle);
// return unadjusted target if already over vehicle's projected maximum speed
if (target_speed >= vehicle_speed_max) {
return target_speed;
}
const float speed_increase_max = vehicle_speed_max - fabsf(target_speed);
float speed_nudge = ((fabsf(pilot_throttle) - 50.0f) / 50.0f) * speed_increase_max;
if (pilot_throttle < 0) {
speed_nudge = -speed_nudge;
}
return target_speed + speed_nudge;
}
// calculated a reduced speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint

14
APMrover2/mode.h
View File

@ -92,10 +92,16 @@ protected:
// calculates the amount of throttle that should be output based
// on things like proximity to corners and current speed
virtual void calc_throttle(float target_speed, bool reversed = false);
virtual void calc_throttle(float target_speed, bool nudge_allowed = true);
// calculate pilot input to nudge throttle up or down
int16_t calc_throttle_nudge();
// estimate maximum vehicle speed (in m/s)
float calc_speed_max(float cruise_speed, float cruise_throttle);
// calculate pilot input to nudge speed up or down
// target_speed should be in meters/sec
// cruise_speed is vehicle's cruising speed, cruise_throttle is the throttle (from -1 to +1) that achieves the cruising speed
// return value is a new speed (in m/s) which up to the projected maximum speed based on the cruise speed and cruise throttle
float calc_speed_nudge(float target_speed, float cruise_speed, float cruise_throttle);
// calculated a reduced speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint
// should be called after calc_lateral_acceleration and before calc_throttle
@ -132,7 +138,7 @@ public:
// methods that affect movement of the vehicle in this mode
void update() override;
void calc_throttle(float target_speed, bool reversed = false);
void calc_throttle(float target_speed, bool nudge_allowed = true);
// attributes of the mode
bool is_autopilot_mode() const override { return true; }

11
APMrover2/mode_auto.cpp
View File

@ -9,8 +9,7 @@ bool ModeAuto::_enter()
return false;
}
// init controllers and location target
g.pidSpeedThrottle.reset_I();
// init location target
set_desired_location(rover.current_loc, false);
// other initialisation
@ -52,7 +51,7 @@ void ModeAuto::update()
// continue driving towards destination
calc_lateral_acceleration(active_at_destination ? rover.current_loc : _origin, _destination, _reversed);
calc_nav_steer(_reversed);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), _reversed);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true);
} else {
// we have reached the destination so stop
g2.motors.set_throttle(g.throttle_min.get());
@ -69,7 +68,7 @@ void ModeAuto::update()
const float yaw_error = wrap_PI(radians((_desired_yaw_cd - ahrs.yaw_sensor) * 0.01f));
const float steering_out = attitude_control.get_steering_out_angle_error(yaw_error, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right);
g2.motors.set_steering(steering_out * 4500.0f);
calc_throttle(_desired_speed);
calc_throttle(_desired_speed, true);
// check if we have reached target
_reached_heading = (fabsf(yaw_error) < radians(5));
} else {
@ -175,7 +174,7 @@ bool ModeAuto::check_trigger(void)
return false;
}
void ModeAuto::calc_throttle(float target_speed, bool reversed)
void ModeAuto::calc_throttle(float target_speed, bool nudge_allowed)
{
// If not autostarting set the throttle to minimum
if (!check_trigger()) {
@ -186,5 +185,5 @@ void ModeAuto::calc_throttle(float target_speed, bool reversed)
}
return;
}
Mode::calc_throttle(target_speed, reversed);
Mode::calc_throttle(target_speed, nudge_allowed);
}

6
APMrover2/mode_guided.cpp
View File

@ -33,7 +33,7 @@ void ModeGuided::update()
// continue driving towards destination
calc_lateral_acceleration(_origin, _destination);
calc_nav_steer();
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed));
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
} else {
// we've reached destination so stop
g2.motors.set_throttle(g.throttle_min.get());
@ -55,7 +55,7 @@ void ModeGuided::update()
const float yaw_error = wrap_PI(radians((_desired_yaw_cd - ahrs.yaw_sensor) * 0.01f));
const float steering_out = attitude_control.get_steering_out_angle_error(yaw_error, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right);
g2.motors.set_steering(steering_out * 4500.0f);
calc_throttle(_desired_speed);
calc_throttle(_desired_speed, true);
} else {
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0.0f);
@ -74,7 +74,7 @@ void ModeGuided::update()
// run steering and throttle controllers
float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f), g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right);
g2.motors.set_steering(steering_out * 4500.0f);
calc_throttle(_desired_speed);
calc_throttle(_desired_speed, true);
} else {
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0.0f);

2
APMrover2/mode_rtl.cpp
View File

@ -32,7 +32,7 @@ void ModeRTL::update()
// continue driving towards destination
calc_lateral_acceleration(_origin, _destination);
calc_nav_steer();
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed));
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
} else {
// we've reached destination so stop
g2.motors.set_throttle(g.throttle_min.get());

22
APMrover2/mode_steering.cpp
View File

@ -3,16 +3,23 @@
void ModeSteering::update()
{
// convert pilot throttle input to desired speed
// speed in proportion to cruise speed, up to 50% throttle, then uses nudging above that.
float target_speed = channel_throttle->get_control_in() * 0.01f * 2.0f * g.speed_cruise;
target_speed = constrain_float(target_speed, -g.speed_cruise, g.speed_cruise);
// convert pilot throttle input to desired speed (up to twice the cruise speed)
float target_speed = channel_throttle->get_control_in() * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise);
// get speed forward
float speed;
if (!attitude_control.get_forward_speed(speed)) {
// no valid speed so so stop
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0.0f);
lateral_acceleration = 0.0f;
return;
}
// in steering mode we control lateral acceleration directly. We first calculate the maximum lateral
// acceleration at full steering lock for this speed. That is V^2/R where R is the radius of turn.
// We get the radius of turn from half the STEER2SRV_P.
const float ground_speed = rover.ground_speed;
float max_g_force = ground_speed * ground_speed / MAX(g2.turn_radius, 0.1f);
float max_g_force = speed * speed / MAX(g2.turn_radius, 0.1f);
// constrain to user set TURN_MAX_G
max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);
@ -25,7 +32,6 @@ void ModeSteering::update()
if (is_negative(target_speed)) {
reversed = true;
lateral_acceleration = -lateral_acceleration;
target_speed = fabsf(target_speed);
}
// mark us as in_reverse when using a negative throttle
@ -35,5 +41,5 @@ void ModeSteering::update()
calc_nav_steer(reversed);
// run speed to throttle output controller
calc_throttle(target_speed, reversed);
calc_throttle(target_speed, false);
}