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

Rover: integrate balancebot pitch limit protection

This commit is contained in:
Randy Mackay 2022-11-10 12:01:20 +09:00
parent 71ae3f0a13
commit f74d22135c
2 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Rover/balance_bot.cpp
View File

@ -8,7 +8,7 @@ void Rover::balancebot_pitch_control(float &throttle)
const float demanded_pitch = radians(-throttle * 0.01f * g2.bal_pitch_max) + radians(g2.bal_pitch_trim);
// calculate required throttle using PID controller
throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, g2.motors.limit.throttle_lower || g2.motors.limit.throttle_upper, G_Dt) * 100.0f;
throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, radians(g2.bal_pitch_max), g2.motors.limit.throttle_lower || g2.motors.limit.throttle_upper, G_Dt) * 100.0f;
}
// returns true if vehicle is a balance bot

4
Rover/mode.cpp
View File

@ -317,7 +317,9 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled)
bool stopped;
throttle_out = 100.0f * attitude_control.get_throttle_out_stop(g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt, stopped);
} else {
throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt);
bool motor_lim_low = g2.motors.limit.throttle_lower || attitude_control.pitch_limited();
bool motor_lim_high = g2.motors.limit.throttle_upper || attitude_control.pitch_limited();
throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, motor_lim_low, motor_lim_high, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt);
}
// if vehicle is balance bot, calculate actual throttle required for balancing