diff --git a/Tools/autotest/antennatracker.py b/Tools/autotest/antennatracker.py index d89ced4f0d69..860f32a85d7e 100644 --- a/Tools/autotest/antennatracker.py +++ b/Tools/autotest/antennatracker.py @@ -141,6 +141,126 @@ def MAV_CMD_DO_SET_SERVO(self): ) self.wait_servo_channel_value(channel, value) + def sim_relative_yaw_pitch(self): + '''return the simulated tracker's yaw and pitch in degrees, + relative to the 270 degree yaw and 10 degree pitch at the centre + of its range''' + m = self.assert_receive_message('SIMSTATE') + return (mavextra.wrap_180(math.degrees(m.yaw) - 270), + math.degrees(m.pitch) - 10) + + def wait_sim_relative_yaw_pitch(self, yaw, pitch, tolerance=1, timeout=120): + '''wait for the simulated tracker to reach a relative yaw and pitch. + Pitch moves at only 1 degree/second, so this can take a while''' + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > timeout: + raise NotAchievedException(f"Did not reach yaw={yaw} pitch={pitch}") + (current_yaw, current_pitch) = self.sim_relative_yaw_pitch() + if (abs(mavextra.wrap_180(current_yaw - yaw)) < tolerance and + abs(current_pitch - pitch) < tolerance): + return + + def UndrivenServosHoldPosition(self): + '''check the simulated tracker does not move while its servos are not driven''' + self.change_mode(0) # "MANUAL" + # centre the tracker so that it has room to move in either direction + self.set_rc(1, 1500) + self.set_rc(2, 1500) + self.wait_sim_relative_yaw_pitch(0, 0) + self.disarm_vehicle() + for channel in 1, 2: + self.wait_servo_channel_value(channel, 0) + (start_yaw, start_pitch) = self.sim_relative_yaw_pitch() + self.delay_sim_time(12, reason="tracker to hold position") + (end_yaw, end_pitch) = self.sim_relative_yaw_pitch() + self.progress(f"yaw {start_yaw:.1f} -> {end_yaw:.1f}, pitch {start_pitch:.1f} -> {end_pitch:.1f}") + if abs(mavextra.wrap_180(end_yaw - start_yaw)) > 1: + raise NotAchievedException("Tracker yawed while undriven") + if abs(end_pitch - start_pitch) > 1: + raise NotAchievedException("Tracker pitched while undriven") + self.arm_vehicle() + + def ServoRangeLimits(self): + '''check the simulated tracker stops at the limits of its range''' + self.set_parameters({ + "SERVO1_MAX": 2200, + "SERVO2_MAX": 2200, + }) + self.change_mode(0) # "MANUAL" + # 2200 asks for 1.4 times the model's +/-170 degree yaw range + # and +/-45 degree pitch range + for channel in 1, 2: + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_SET_SERVO, + p1=channel, + p2=2200, + ) + self.wait_servo_channel_value(channel, 2200) + + self.wait_sim_relative_yaw_pitch(170, 45) + + tstart = self.get_sim_time() + while self.get_sim_time_cached() - tstart < 10: + (yaw, pitch) = self.sim_relative_yaw_pitch() + if not 169 < yaw < 171: + raise NotAchievedException(f"Tracker yaw {yaw:.1f} not held at limit") + if not 44 < pitch < 46: + raise NotAchievedException(f"Tracker pitch {pitch:.1f} not held at limit") + + def Tracking(self): + '''check the tracker points at a vehicle sending its position''' + self.change_mode("AUTO") + self.wait_ready_to_arm() + + m = self.assert_receive_message('GLOBAL_POSITION_INT') + tracker_loc = Location(m.lat * 1.0e-7, m.lon * 1.0e-7, m.alt * 1.0e-3, AltFrame.ABSOLUTE) + # the vehicle reports the tracker's own pressure, so the + # tracker sees no altitude difference and should pitch to zero + m = self.assert_receive_message('SCALED_PRESSURE') + vehicle_press_abs = m.press_abs + vehicle_temperature = m.temperature + + vehicle_loc = None + last_sent = 0 + + def send_vehicle_position(mav, m): + nonlocal last_sent + now = self.get_sim_time_cached() + if vehicle_loc is None or now - last_sent < 0.1: + return + last_sent = now + self.mav.mav.global_position_int_send( + int(now * 1000), # time_boot_ms + int(vehicle_loc.lat * 1e7), + int(vehicle_loc.lng * 1e7), + int(vehicle_loc.get_alt_m(AltFrame.ABSOLUTE) * 1000), + 0, # relative_alt + 0, # vx + 0, # vy + 0, # vz + 65535 # hdg + ) + self.mav.mav.scaled_pressure_send( + int(now * 1000), # time_boot_ms + vehicle_press_abs, + 0, # press_diff + vehicle_temperature + ) + + self.context_push() + self.install_message_hook_context(send_vehicle_position) + + # the simulated tracker's yaw range is centred on west and + # spans 170 degrees either way, so keep the vehicle to the west + for (north, east) in (0, -20), (20, -20), (-20, -20), (30, -40), (-30, -40): + desyaw = math.degrees(math.atan2(east, north)) + self.start_subtest(f"Vehicle {north}m north {east}m east (bearing {desyaw:.0f})") + vehicle_loc = self.offset_location_ne(tracker_loc, north, east) + self.wait_attitude(despitch=0, desyaw=desyaw, tolerance=2, timeout=120, minimum_duration=5) + + self.context_pop() + def MAV_CMD_MISSION_START(self): '''test MAV_CMD_MISSION_START mavlink command''' for method in self.run_cmd, self.run_cmd_int: @@ -268,5 +388,8 @@ def tests(self): self.GPSForYaw, self.LoggerMsgChunks, self.StationaryGlobalPositionIntAlt, + self.UndrivenServosHoldPosition, + self.ServoRangeLimits, + self.Tracking, ]) return ret diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 8214bae083ef..dbb4e9db2cfd 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -6475,28 +6475,40 @@ def wait_disarmed(self, timeout=None, tstart=None): (delta, timeout)) return - def wait_attitude(self, desroll=None, despitch=None, timeout=2, tolerance=10, - message_type='ATTITUDE', use_cached_simtime=False): - '''wait for an attitude (degrees)''' - if desroll is None and despitch is None: - raise ValueError("despitch or desroll must be supplied") + def wait_attitude(self, desroll=None, despitch=None, desyaw=None, timeout=2, tolerance=10, + message_type='ATTITUDE', use_cached_simtime=False, minimum_duration=None): + '''wait for an attitude (degrees). If minimum_duration is + supplied the attitude must be held for that many seconds''' + if desroll is None and despitch is None and desyaw is None: + raise ValueError("desroll, despitch or desyaw must be supplied") + if minimum_duration is not None and minimum_duration >= timeout: + raise ValueError("minimum_duration must be less than timeout") if use_cached_simtime: tstart = self.get_sim_time() else: tstart = self.get_sim_time_cached() + achieved_start = None while True: if self.get_sim_time_cached() - tstart > timeout: raise AutoTestTimeoutException("Failed to achieve attitude") m = self.assert_receive_message(message_type, timeout=60) roll_deg = math.degrees(m.roll) pitch_deg = math.degrees(m.pitch) - self.progress("wait_att[%s]: roll=%f desroll=%s pitch=%f despitch=%s" % - (message_type, roll_deg, desroll, pitch_deg, despitch)) - if desroll is not None and abs(roll_deg - desroll) > tolerance: + yaw_deg = math.degrees(m.yaw) + self.progress("wait_att[%s]: roll=%f desroll=%s pitch=%f despitch=%s yaw=%f desyaw=%s" % + (message_type, roll_deg, desroll, pitch_deg, despitch, yaw_deg, desyaw)) + if ((desroll is not None and abs(roll_deg - desroll) > tolerance) or + (despitch is not None and abs(pitch_deg - despitch) > tolerance) or + (desyaw is not None and self.heading_delta(yaw_deg, desyaw) > tolerance)): + achieved_start = None continue - if despitch is not None and abs(pitch_deg - despitch) > tolerance: - continue - return + if minimum_duration is None: + return + now = self.get_sim_time_cached() + if achieved_start is None: + achieved_start = now + if now - achieved_start >= minimum_duration: + return def wait_attitude_quaternion(self, desroll=None, diff --git a/libraries/SITL/SIM_Tracker.cpp b/libraries/SITL/SIM_Tracker.cpp index d70c1240cabc..b5b562af65b8 100644 --- a/libraries/SITL/SIM_Tracker.cpp +++ b/libraries/SITL/SIM_Tracker.cpp @@ -68,10 +68,19 @@ void Tracker::update(const struct sitl_input &input) float yaw_rate = 0.0f, pitch_rate = 0.0f; - yaw_input = (input.servos[0]-1500)/500.0f; - pitch_input = (input.servos[1]-1500)/500.0f; + // a zero PWM means the channel is not being driven at all, which is what + // the vehicle code emits while the servos are not being output to. Taking + // that as a demand normalises it to -3.0, commanding the axis hard to one + // end of its range; hold position instead, as an undriven servo would + const bool yaw_driven = input.servos[0] != 0; + const bool pitch_driven = input.servos[1] != 0; + if (yaw_driven) { + yaw_input = (input.servos[0]-1500)/500.0f; + } + if (pitch_driven) { + pitch_input = (input.servos[1]-1500)/500.0f; + } - // implement yaw and pitch limits float r, p, y; dcm.to_euler(&r, &p, &y); @@ -84,6 +93,22 @@ void Tracker::update(const struct sitl_input &input) if (yaw_current_relative < -180) { yaw_current_relative += 360; } + + if (onoff) { + update_onoff_servos(yaw_rate, pitch_rate); + } else { + update_position_servos(delta_time, yaw_rate, pitch_rate); + } + + if (!yaw_driven) { + yaw_rate = 0; + } + if (!pitch_driven) { + pitch_rate = 0; + } + + // implement the yaw and pitch limits. These must be applied after the + // servo update, which is what sets the rates if (yaw_rate > 0 && yaw_current_relative >= yaw_range) { yaw_rate = 0; } @@ -97,13 +122,6 @@ void Tracker::update(const struct sitl_input &input) pitch_rate = 0; } - if (onoff) { - update_onoff_servos(yaw_rate, pitch_rate); - } else { - update_position_servos(delta_time, yaw_rate, pitch_rate); - } - - // keep it level float roll_rate = 0 - roll_current; diff --git a/libraries/SITL/SIM_Tracker.h b/libraries/SITL/SIM_Tracker.h index 2ff7ecaba459..5025f2baf571 100644 --- a/libraries/SITL/SIM_Tracker.h +++ b/libraries/SITL/SIM_Tracker.h @@ -43,8 +43,8 @@ class Tracker : public Aircraft { const float pitchrate = 1.0f; const float pitch_range = 45; const float yaw_range = 170; - const float zero_yaw = 270; // yaw direction at startup - const float zero_pitch = 10; // pitch at startup + const float zero_yaw = 270; // yaw at the centre of the yaw range + const float zero_pitch = 10; // pitch at the centre of the pitch range uint64_t last_debug_us; float pitch_input;