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

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

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) {

27
ArduPlane/Plane.h
View File

@ -311,7 +311,7 @@ private:
// last time we ran roll/pitch stabilization
uint32_t last_stabilize_ms;
// Failsafe
struct {
// Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold
@ -336,10 +336,10 @@ private:
// the time when the last HEARTBEAT message arrived from a GCS
uint32_t last_heartbeat_ms;
// A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal
uint32_t short_timer_ms;
uint32_t last_valid_rc_ms;
//keeps track of the last valid rc as it relates to the AFS system
@ -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;
@ -428,7 +429,7 @@ private:
// this is a 0..36000 value, or -1 for disabled
int32_t hold_course_cd = -1;
// locked_course and locked_course_cd are used in stabilize mode
// locked_course and locked_course_cd are used in stabilize mode
// when ground steering is active, and for steering in auto-takeoff
bool locked_course;
float locked_course_err;
@ -446,7 +447,7 @@ private:
// the highest airspeed we have reached since entering AUTO. Used
// to control ground takeoff
float highest_airspeed;
// turn angle for next leg of mission
float next_turn_angle {90};
@ -455,13 +456,13 @@ private:
// time when we first pass min GPS speed on takeoff
uint32_t takeoff_speed_time_ms;
// distance to next waypoint
float wp_distance;
// proportion to next waypoint
float wp_proportion;
// last time is_flying() returned true in milliseconds
uint32_t last_flying_ms;
@ -556,7 +557,7 @@ private:
AP_Vehicle::FixedWing::FlightStage last_flight_stage;
} gear;
#endif
struct {
// on hard landings, only check once after directly a landing so you
// don't trigger a crash when picking up the aircraft
@ -580,7 +581,7 @@ private:
// this controls throttle suppression in auto modes
bool throttle_suppressed;
// reduce throttle to eliminate battery over-current
int8_t throttle_watt_limit_max;
int8_t throttle_watt_limit_min; // for reverse thrust
@ -654,7 +655,7 @@ private:
// previous target bearing, used to update sum_cd
int32_t old_target_bearing_cd;
// Total desired rotation in a loiter. Used for Loiter Turns commands.
// Total desired rotation in a loiter. Used for Loiter Turns commands.
int32_t total_cd;
// total angle completed in the loiter so far
@ -951,7 +952,7 @@ private:
void three_hz_loop(void);
#if AP_AIRSPEED_AUTOCAL_ENABLE
void airspeed_ratio_update(void);
#endif
#endif
void compass_save(void);
void update_logging1(void);
void update_logging2(void);
@ -1099,7 +1100,7 @@ private:
bool ekf_over_threshold();
void failsafe_ekf_event();
void failsafe_ekf_off_event(void);
enum class CrowMode {
NORMAL,
PROGRESSIVE,

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);
}

52
ArduPlane/navigation.cpp
View File

@ -130,7 +130,7 @@ void Plane::navigate()
void Plane::calc_airspeed_errors()
{
float airspeed_measured = 0;
// we use the airspeed estimate function not direct sensor as TECS
// may be using synthetic airspeed
ahrs.airspeed_estimate(airspeed_measured);
@ -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;
}
@ -215,7 +224,7 @@ void Plane::calc_airspeed_errors()
// when using the special GUIDED mode features for slew control, don't allow airspeed nudging as it doesn't play nicely.
#if OFFBOARD_GUIDED == ENABLED
if (control_mode == &mode_guided && !is_zero(guided_state.target_airspeed_cm) && (airspeed_nudge_cm != 0)) {
if (control_mode == &mode_guided && !is_zero(guided_state.target_airspeed_cm) && (airspeed_nudge_cm != 0)) {
airspeed_nudge_cm = 0; //airspeed_nudge_cm forced to zero
}
#endif
@ -308,7 +317,7 @@ void Plane::update_loiter(uint16_t radius)
}
/*
handle speed and height control in FBWB or CRUISE mode.
handle speed and height control in FBWB or CRUISE mode.
In this mode the elevator is used to change target altitude. The
throttle is used to change target airspeed or throttle
*/
@ -322,16 +331,16 @@ void Plane::update_fbwb_speed_height(void)
dt = constrain_float(dt, 0.1, 0.15);
target_altitude.last_elev_check_us = now;
float elevator_input = channel_pitch->get_control_in() / 4500.0f;
if (g.flybywire_elev_reverse) {
elevator_input = -elevator_input;
}
int32_t alt_change_cm = g.flybywire_climb_rate * elevator_input * dt * 100;
change_target_altitude(alt_change_cm);
if (is_zero(elevator_input) && !is_zero(target_altitude.last_elevator_input)) {
// the user has just released the elevator, lock in
// the current altitude
@ -350,14 +359,14 @@ void Plane::update_fbwb_speed_height(void)
}
}
#endif
target_altitude.last_elevator_input = elevator_input;
}
check_fbwb_minimum_altitude();
altitude_error_cm = calc_altitude_error_cm();
calc_throttle();
calc_nav_pitch();
}
@ -378,7 +387,7 @@ void Plane::setup_turn_angle(void)
// work out the angle we need to turn through
auto_state.next_turn_angle = wrap_180_cd(next_ground_course_cd - ground_course_cd) * 0.01f;
}
}
}
/*
see if we have reached our loiter target
@ -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()