diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index d8a7f679dadf2..83f03306c2dfd 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -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) { diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 8a93eb533b407..491414ae5db08 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -311,7 +311,7 @@ class Plane : public AP_Vehicle { // 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 @@ class Plane : public AP_Vehicle { // 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 @@ class Plane : public AP_Vehicle { // 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 @@ class Plane : public AP_Vehicle { // 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 @@ class Plane : public AP_Vehicle { // 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 @@ class Plane : public AP_Vehicle { // 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 @@ class Plane : public AP_Vehicle { 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 @@ class Plane : public AP_Vehicle { // 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 @@ class Plane : public AP_Vehicle { // 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 @@ class Plane : public AP_Vehicle { 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 @@ class Plane : public AP_Vehicle { bool ekf_over_threshold(); void failsafe_ekf_event(); void failsafe_ekf_off_event(void); - + enum class CrowMode { NORMAL, PROGRESSIVE, diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index fdb8c78a95bdc..24e8bc3b07a45 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -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; } diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 8c1897e289808..a8be2360d807c 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -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) { diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index cf67a0d4e01b2..06b0d3d00295a 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -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); } - diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 108f527129dce..b2d23dc892533 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -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(); } - diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 76b5047d23bd7..5e7344a339b26 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -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 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index c2994de9fa5b4..f311250b25b15 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -2438,7 +2438,7 @@ def change_speed(): 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 @@ def test_fence_breached_change_mode(self): 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 @@ def test_fence_breach_no_return_point(self): 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()