Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

Plane: fix DO_CHANGE_SPEED airspeed to impact only AUTO and GUIDED modes

Browse Source
This commit is contained in:
Hwurzburg 2021-02-23 10:32:06 -06:00 committed by Andrew Tridgell
parent c5b1c832ba
commit d15e01d390
8 changed files with 52 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/AP_Arming.cpp
View File

@ -248,9 +248,6 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec
//only log if disarming was successful
change_arm_state();
// reload target airspeed which could have been modified by a mission
plane.aparm.airspeed_cruise_cm.load();
#if QAUTOTUNE_ENABLED
//save qautotune gains if enabled and success
if (plane.control_mode == &plane.mode_qautotune) {

1
ArduPlane/Plane.h
View File

@ -376,6 +376,7 @@ private:
// The calculated airspeed to use in FBW-B. Also used in higher modes for insuring min ground speed is met.
// Also used for flap deployment criteria. Centimeters per second.
int32_t target_airspeed_cm;
int32_t new_airspeed_cm = -1; //temp variable for AUTO and GUIDED mode speed changes
// The difference between current and desired airspeed. Used in the pitch controller. Meters per second.
float airspeed_error;

2
ArduPlane/commands_logic.cpp
View File

@ -880,7 +880,7 @@ bool Plane::do_change_speed(const AP_Mission::Mission_Command& cmd)
{
case 0: // Airspeed
if ((cmd.content.speed.target_ms >= aparm.airspeed_min.get()) && (cmd.content.speed.target_ms <= aparm.airspeed_max.get())) {
aparm.airspeed_cruise_cm.set(cmd.content.speed.target_ms * 100);
new_airspeed_cm = cmd.content.speed.target_ms * 100; //new airspeed target for AUTO or GUIDED modes
gcs().send_text(MAV_SEVERITY_INFO, "Set airspeed %u m/s", (unsigned)cmd.content.speed.target_ms);
return true;
}

3
ArduPlane/mode.cpp
View File

@ -64,6 +64,9 @@ bool Mode::enter()
plane.auto_state.vtol_mode = false;
plane.auto_state.vtol_loiter = false;
// initialize speed variable used in AUTO and GUIDED for DO_CHANGE_SPEED commands
plane.new_airspeed_cm = -1;
bool enter_result = _enter();
if (enter_result) {

2
ArduPlane/mode_guided.cpp
View File

@ -10,7 +10,6 @@ bool ModeGuided::_enter()
*/
plane.guided_WP_loc = plane.current_loc;
plane.set_guided_WP();
return true;
}
@ -30,4 +29,3 @@ void ModeGuided::navigate()
// Zero indicates to use WP_LOITER_RAD
plane.update_loiter(0);
}

32
ArduPlane/navigation.cpp
View File

@ -166,7 +166,7 @@ void Plane::calc_airspeed_errors()
get_throttle_input()) + ((int32_t)aparm.airspeed_min * 100);
}
#if OFFBOARD_GUIDED == ENABLED
} else if (control_mode == &mode_guided && !is_zero(guided_state.target_airspeed_cm)) {
} else if (control_mode == &mode_guided && guided_state.target_airspeed_cm > 0.0) { // if offbd guided speed change cmd not set, then this section is skipped
// offboard airspeed demanded
uint32_t now = AP_HAL::millis();
float delta = 1e-3f * (now - guided_state.target_airspeed_time_ms);
@ -182,22 +182,31 @@ void Plane::calc_airspeed_errors()
}
#endif // OFFBOARD_GUIDED == ENABLED
} else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
// Landing airspeed target
target_airspeed_cm = landing.get_target_airspeed_cm();
} else if ((control_mode == &mode_auto) &&
(quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) &&
((vtol_approach_s.approach_stage == Landing_ApproachStage::APPROACH_LINE) ||
(vtol_approach_s.approach_stage == Landing_ApproachStage::VTOL_LANDING))) {
const float land_airspeed = SpdHgt_Controller->get_land_airspeed();
if (is_positive(land_airspeed)) {
target_airspeed_cm = land_airspeed * 100;
} else if (control_mode == &mode_guided && new_airspeed_cm > 0) { //DO_CHANGE_SPEED overrides onboard guided speed commands, user would have re-enter guided mode to revert
target_airspeed_cm = new_airspeed_cm;
} else if (control_mode == &mode_auto) {
if ((quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) &&
((vtol_approach_s.approach_stage == Landing_ApproachStage::APPROACH_LINE) ||
(vtol_approach_s.approach_stage == Landing_ApproachStage::VTOL_LANDING))) {
const float land_airspeed = SpdHgt_Controller->get_land_airspeed();
if (is_positive(land_airspeed)) {
target_airspeed_cm = land_airspeed * 100;
} else {
// fallover to normal airspeed
target_airspeed_cm = aparm.airspeed_cruise_cm;
}
} else {
// fallover to normal airspeed
target_airspeed_cm = aparm.airspeed_cruise_cm;
// normal AUTO mode and new_airspeed variable was set by DO_CHANGE_SPEED command while in AUTO mode
if (new_airspeed_cm > 0) {
target_airspeed_cm = new_airspeed_cm;
}
}
} else {
// Normal airspeed target
// Normal airspeed target for all other cases
target_airspeed_cm = aparm.airspeed_cruise_cm;
}
@ -390,4 +399,3 @@ bool Plane::reached_loiter_target(void)
}
return nav_controller->reached_loiter_target();
}

2
ArduPlane/quadplane.cpp
View File

@ -3005,8 +3005,6 @@ bool QuadPlane::check_land_complete(void)
if (land_detector(4000)) {
poscontrol.state = QPOS_LAND_COMPLETE;
gcs().send_text(MAV_SEVERITY_INFO,"Land complete");
// reload target airspeed which could have been modified by the mission
plane.aparm.airspeed_cruise_cm.load();
if (plane.control_mode != &plane.mode_auto ||
!plane.mission.continue_after_land()) {
// disarm on land unless we have MIS_OPTIONS setup to

8
Tools/autotest/arduplane.py
View File

@ -2438,7 +2438,7 @@ class AutoTestPlane(AutoTest):
50 # alt
)
self.delay_sim_time(5)
new_target_groundspeed = m.groundspeed + 5
new_target_groundspeed = m.groundspeed + 10
self.run_cmd(
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
1, # groundspeed
@ -2552,7 +2552,7 @@ class AutoTestPlane(AutoTest):
self.wait_ready_to_arm()
self.takeoff(alt=50)
self.change_mode("CRUISE")
self.wait_distance(150, accuracy=20)
self.wait_distance(90, accuracy=15)
self.progress("Enable fence and initiate fence action")
self.do_fence_enable()
@ -2633,8 +2633,8 @@ class AutoTestPlane(AutoTest):
self.progress("Return loc: (%s)" % str(ret_loc))
# Wait for guided return to vehicle calculated fence return location
self.wait_distance_to_location(ret_loc, 105, 115)
self.wait_circling_point_with_radius(ret_loc, want_radius)
self.wait_distance_to_location(ret_loc, 90, 110)
self.wait_circling_point_with_radius(ret_loc, 92)
self.progress("Test complete, disable fence and come home")
self.do_fence_disable()