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

Rover: move throttle nudge to Mode class

This commit is contained in:
Randy Mackay 2017-07-18 16:27:05 +09:00
parent b9644c4ec7
commit f17f56dea4
4 changed files with 22 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
APMrover2/Rover.h
View File

@ -271,9 +271,6 @@ private:
// ground speed error in m/s
float groundspeed_error;
// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
int16_t throttle_nudge;
// receiver RSSI
uint8_t receiver_rssi;

21
APMrover2/mode.cpp
View File

@ -31,7 +31,6 @@ bool Mode::enter()
void Mode::calc_throttle(float target_speed)
{
const int16_t &throttle_nudge = rover.throttle_nudge;
int16_t &throttle = rover.throttle;
const int32_t next_navigation_leg_cd = rover.next_navigation_leg_cd;
const AP_AHRS &ahrs = rover.ahrs;
@ -40,7 +39,7 @@ void Mode::calc_throttle(float target_speed)
const float ground_speed = rover.ground_speed;
const float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise;
const int throttle_target = throttle_base + throttle_nudge;
const int throttle_target = throttle_base + calc_throttle_nudge();
/*
reduce target speed in proportion to turning rate, up to the
@ -112,6 +111,24 @@ void Mode::calc_lateral_acceleration()
calc_lateral_acceleration(rover.current_loc, rover.next_WP);
}
// calculate pilot input to nudge throttle up or down
int16_t Mode::calc_throttle_nudge()
{
// get pilot throttle input (-100 to +100)
int16_t pilot_throttle = rover.channel_throttle->get_control_in();
int16_t throttle_nudge = 0;
// 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);
}
return throttle_nudge;
}
/*
* Calculate desired turn angles (in medium freq loop)
*/

3
APMrover2/mode.h
View File

@ -82,6 +82,9 @@ protected:
// calculate desired lateral acceleration
void calc_lateral_acceleration(const struct Location &last_wp, const struct Location &next_WP);
// calculate pilot input to nudge throttle up or down
int16_t calc_throttle_nudge();
// references to avoid code churn:
class Parameters &g;
class ParametersG2 &g2;

11
APMrover2/radio.cpp
View File

@ -156,17 +156,6 @@ void Rover::read_radio()
g2.motors.set_throttle(channel_throttle->get_control_in());
g2.motors.set_steering(channel_steer->get_control_in());
// 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(g2.motors.get_throttle()) > 50.0f) &&
((is_negative(g2.motors.get_throttle()) && in_reverse) ||
(is_positive(g2.motors.get_throttle()) && !in_reverse))) {
throttle_nudge = (g.throttle_max - g.throttle_cruise) *
((fabsf(channel_throttle->norm_input()) - 0.5f) / 0.5f);
} else {
throttle_nudge = 0;
}
// check if we try to do RC arm/disarm
rudder_arm_disarm_check();
}