diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 11b2a64410c4c..9637c5a511c94 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -750,6 +750,10 @@ class ModeAuto : public Mode { float up; // desired speed upwards in m/s. 0 if unset float down; // desired speed downwards in m/s. 0 if unset } desired_speed_override; + + // method to process do_change_speed before we calculate SCurve: + void do_change_speed_lookahead(uint16_t index); + }; #if AUTOTUNE_ENABLED == ENABLED diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 0da35567d39a9..81b588362f568 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1539,6 +1539,8 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) // get waypoint's location from command and send to wp_nav const Location target_loc = loc_from_cmd(cmd, default_loc); + do_change_speed_lookahead(cmd.index+1); + if (!wp_start(target_loc)) { // failure to set next destination can only be because of missing terrain data copter.failsafe_terrain_on_event(); @@ -1558,6 +1560,21 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) } } +// if a DO_CHANGE_SPEED is found then process it immediately. This +// prevents the vehicle creating an accel segment before it processes +// the command. +void ModeAuto::do_change_speed_lookahead(uint16_t index) +{ + AP_Mission::Mission_Command cmd; + if (!mission.get_next_do_cmd(index, cmd)) { + return; + } + if (cmd.id != MAV_CMD_DO_CHANGE_SPEED) { + return; + } + do_change_speed(cmd); +} + // checks the next mission command and adds it as a destination if necessary // supports both straight line and spline waypoints // cmd should be the current command @@ -1915,6 +1932,7 @@ void ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd) // Do (Now) commands /********************************************************************************/ +// note that we extract a lot of this same information in segment_speed void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd) { if (cmd.content.speed.target_ms > 0) { diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 638fe6ffdf683..67930326fe74a 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -10007,6 +10007,71 @@ def DO_CHANGE_SPEED(self): self.wait_disarmed() + def DO_CHANGE_SPEED_SCurve(self): + '''make sure speed is honoured quickly''' + alt = 10 + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, alt), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 40, 0, alt), + self.create_MISSION_ITEM_INT(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, p2=1.5), + (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), + ]) + self.set_parameter('AUTO_OPTIONS', 3) + self.change_mode('AUTO') + self.wait_ready_to_arm() + + self.context_push() + self.progress("Installing hook to watch maximum speed") + lg = vehicle_test_suite.TestSuite.ValidateGroundSpeed(self, 0, 3) + self.install_message_hook_context(lg) + self.arm_vehicle() + self.progress("Wait for vehicle to start moving") + self.wait_groundspeed(0.75, 10) + self.progress("Ensuring vehicle does not slow back down again") + mg = vehicle_test_suite.TestSuite.ValidateGroundSpeed(self, 0.5, 1.75) + self.install_message_hook_context(mg) + + # RTL speeds up, so we stop watching the speed when we are entering it + self.wait_current_waypoint(2, timeout=10) + self.wait_distance_to_nav_target(10, 20, timeout=300) + self.context_pop() + self.wait_current_waypoint(4, timeout=300) + + self.wait_disarmed(timeout=300) + + def DO_CHANGE_SPEED_SCurveNext(self): + '''make sure speed is honoured quickly for waypoint past first''' + alt = 10 + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, alt), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 40, 0, alt), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 80, 0, alt), + self.create_MISSION_ITEM_INT(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, p2=1.5), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 180, 0, alt), + (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), + ]) + self.set_parameter('AUTO_OPTIONS', 3) + self.change_mode('AUTO') + self.wait_ready_to_arm() + + self.context_push() + self.arm_vehicle() + # this is the 0-40m segment: + self.wait_groundspeed(6, 8) + self.progress("Ensuring vehicle does not slow back down again") + mg = vehicle_test_suite.TestSuite.ValidateGroundSpeed(self, 0.5, 10) + self.install_message_hook_context(mg) + + # this is the 80-180m segment: + self.wait_groundspeed(1.4, 1.6) + self.assert_current_waypoint(3) + + # RTL speeds up, so we stop watching the speed when we are entering it + self.wait_distance_to_nav_target(10, 20, timeout=300) + self.context_pop() + + self.wait_disarmed(timeout=300) + def AUTO_LAND_TO_BRAKE(self): '''ensure terrain altitude is taken into account when braking''' self.set_parameters({ @@ -11769,6 +11834,8 @@ def tests2b(self): # this block currently around 9.5mins here self.GroundEffectCompensation_touchDownExpected, self.GroundEffectCompensation_takeOffExpected, self.DO_CHANGE_SPEED, + self.DO_CHANGE_SPEED_SCurve, + self.DO_CHANGE_SPEED_SCurveNext, self.MISSION_START, self.AUTO_LAND_TO_BRAKE, self.WPNAV_SPEED, diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index f4cdceaf44412..0eb2c615f59d0 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -3521,6 +3521,37 @@ class ValidateAHRS3AgainstSimState(ValidateIntPositionAgainstSimState): def __init__(self, suite, **kwargs): super(TestSuite.ValidateAHRS3AgainstSimState, self).__init__(suite, 'AHRS3', **kwargs) + class ValidateRange(MessageHook): + '''monitors vehicle Groundspeed and snure it stays between limits''' + def __init__(self, suite, minimum_value, maximum_value): + super(TestSuite.ValidateRange, self).__init__(suite) + self.minimum_value = minimum_value + self.maximum_value = maximum_value + self.last_print = 0 + self.min_print_interval = 1 # seconds + + def process_value(self, value): + if time.time() - self.last_print > self.min_print_interval: + self.progress(f"{self.progress_prefix()}: groundspeed (min={self.minimum_value} <= current={value} <= {self.maximum_value})") # noqa:501 + self.last_print = time.time() + + if value < self.minimum_value: + raise NotAchievedException(f"Value {value} below minimum {self.minimum_value}") + if value > self.maximum_value: + raise NotAchievedException(f"Value {value} above maximum {self.maximum_value}") + + def hook_removed(self): + self.progress("groundspeed was maintained") + + class ValidateGroundSpeed(ValidateRange): + def process(self, mav, m): + if m.get_type() != "VFR_HUD": + return + self.process_value(m.groundspeed) + + def progress_prefix(self): + return "VGS: " + def message_hook(self, mav, msg): """Called as each mavlink msg is received.""" # print("msg: %s" % str(msg)) diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 32bb2fa77dd8d..c3ada90c405bb 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -778,6 +778,11 @@ class AP_Mission } #endif + /// returns true if found, false if not found + /// stops and returns false if it hits another navigation command before it finds the first do or conditional command + /// accounts for do_jump commands but never increments the jump's num_times_run (get_next_nav_cmd is responsible for this) + bool get_next_do_cmd(uint16_t start_index, Mission_Command& cmd); + private: static AP_Mission *_singleton; @@ -832,11 +837,6 @@ class AP_Mission bool get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool increment_jump_num_times_if_found, bool send_gcs_msg = true); /// get_next_do_cmd - gets next "do" or "conditional" command after start_index - /// returns true if found, false if not found - /// stops and returns false if it hits another navigation command before it finds the first do or conditional command - /// accounts for do_jump commands but never increments the jump's num_times_run (get_next_nav_cmd is responsible for this) - bool get_next_do_cmd(uint16_t start_index, Mission_Command& cmd); - /// /// jump handling methods ///