Add Tracker tracking test

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

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,

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;
 

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;