Tools: Add prop blimp support and auto mode

Tools/autotest/blimp.py

@@ -7,10 +7,13 @@
 from __future__ import print_function
 import os
 import shutil
+import math
 
 from pymavlink import mavutil
 
 from vehicle_test_suite import TestSuite
+from pysim import vehicleinfo
+from vehicle_test_suite import NotAchievedException
 
 # get location of scripts
 testdir = os.path.dirname(os.path.realpath(__file__))
@@ -246,6 +249,77 @@ def PREFLIGHT_Pressure(self):
             p3=1, # p3, baro
         )
 
+    def FlyEachFrame(self):
+        '''Fly each supported internal frame'''
+        vinfo = vehicleinfo.VehicleInfo()
+        vinfo_options = vinfo.options[self.vehicleinfo_key()]
+        known_broken_frames = {
+            'none': 'none',
+        }
+        for frame in sorted(vinfo_options["frames"].keys()):
+            if frame in known_broken_frames:
+                self.progress("Not testing known-broken frame (%s)" %
+                              (known_broken_frames[frame]))
+                continue
+            frame_bits = vinfo_options["frames"][frame]
+            print("frame_bits: %s" % str(frame_bits))
+            if frame_bits.get("external", False):
+                self.progress("Not testing external simulation frame (%s)" %
+                              (known_broken_frames[frame]))
+                continue
+
+            self.start_subtest("Testing frame (%s)" % str(frame))
+
+            model = frame_bits.get("model", frame)
+            defaults = self.model_defaults_filepath(frame)
+            if not isinstance(defaults, list):
+                defaults = [defaults]
+            self.customise_SITL_commandline(
+                [],
+                defaults_filepath=defaults,
+                model=model,
+                wipe=True,
+            )
+
+            # add a listener that verifies yaw looks good:
+            def verify_yaw(mav, m):
+                if m.get_type() != 'ATTITUDE':
+                    return
+                yawspeed_thresh_rads = math.radians(20)
+                if m.yawspeed > yawspeed_thresh_rads:
+                    raise NotAchievedException("Excessive yaw on takeoff: %f deg/s > %f deg/s (frame=%s)" %
+                                               (math.degrees(m.yawspeed), math.degrees(yawspeed_thresh_rads), frame))
+            self.context_push()
+            self.install_message_hook_context(verify_yaw)
+            self.set_rc(3,2000)
+            self.wait_altitude(3, 3.5, relative=True, timeout=60)
+            self.set_rc(3, 1500)
+            self.context_pop()
+            self.change_mode('VELOCITY')
+            self.delay_sim_time(1)
+
+            def verify_rollpitch(mav, m):
+                if m.get_type() != 'ATTITUDE':
+                    return
+                pitch_thresh_rad = math.radians(2)
+                if m.pitch > pitch_thresh_rad:
+                    raise NotAchievedException("Excessive pitch %f deg > %f deg" %
+                                               (math.degrees(m.pitch), math.degrees(pitch_thresh_rad)))
+                roll_thresh_rad = math.radians(2)
+                if m.roll > roll_thresh_rad:
+                    raise NotAchievedException("Excessive roll %f deg > %f deg" %
+                                               (math.degrees(m.roll), math.degrees(roll_thresh_rad)))
+            self.context_push()
+            self.install_message_hook_context(verify_rollpitch)
+            for i in range(5):
+                self.set_rc(4, 2000)
+                self.delay_sim_time(0.5)
+                self.set_rc(4, 1500)
+                self.delay_sim_time(5)
+            self.context_pop()
+
+            self.do_RTL()
+
     def tests(self):
         '''return list of all tests'''
         # ret = super(AutoTestBlimp, self).tests()
@@ -254,6 +328,7 @@ def tests(self):
             self.FlyManual,
             self.FlyLoiter,
             self.PREFLIGHT_Pressure,
+            self.FlyEachFrame,
         ])
         return ret
 

Tools/autotest/default_params/blimp-motor.parm

@@ -0,0 +1,66 @@
+FRAME_CLASS 2
+
+SERVO1_MAX       2000
+SERVO1_MIN       1000
+SERVO1_TRIM      1500
+SERVO2_MAX       2000
+SERVO2_MIN       1000
+SERVO2_TRIM      1500
+SERVO3_MAX       2000
+SERVO3_MIN       1000
+SERVO3_TRIM      1500
+SERVO4_MAX       2000
+SERVO4_MIN       1000
+SERVO4_TRIM      1500
+
+LOIT_POS_LAG         0.1
+
+# default PID params for position and velocity-controlled modes
+LOIT_MAX_POS_X   2.0
+LOIT_MAX_POS_Y   2.0
+LOIT_MAX_POS_YAW 2.0
+LOIT_MAX_POS_Z   2.0
+LOIT_MAX_VEL_X   0.3
+LOIT_MAX_VEL_Y   0.3
+LOIT_MAX_VEL_YAW 0.3
+LOIT_MAX_VEL_Z   0.3
+
+LOIT_VELX_D          1.0
+LOIT_VELX_FF         0.0
+LOIT_VELX_I          1.5
+LOIT_VELX_P          5.0
+
+LOIT_VELY_D          1.0
+LOIT_VELY_FF         0.0
+LOIT_VELY_I          1.5
+LOIT_VELY_P          5.0
+
+LOIT_VELYAW_D         0.0
+LOIT_VELYAW_FF        0.0
+LOIT_VELYAW_I         0.8
+LOIT_VELYAW_P         10.0
+
+LOIT_VELZ_D           0.0
+LOIT_VELZ_FF          0.0
+LOIT_VELZ_I           1.0
+LOIT_VELZ_P           15.0
+
+LOIT_POSX_D          0.0
+LOIT_POSX_FF         0.0
+LOIT_POSX_I          0.05
+LOIT_POSX_P          1.0
+
+LOIT_POSY_D          0.0
+LOIT_POSY_FF         0.0
+LOIT_POSY_I          0.05
+LOIT_POSY_P          1.0
+
+LOIT_POSYAW_D         1.0
+LOIT_POSYAW_FF        0.0
+LOIT_POSYAW_I         0.1
+LOIT_POSYAW_P         3.0
+
+LOIT_POSZ_D           0.0
+LOIT_POSZ_FF          0.0
+LOIT_POSZ_I           0.0
+LOIT_POSZ_P           0.7

Tools/autotest/default_params/blimp.parm

@@ -92,55 +92,60 @@ SIM_SERVO_SPEED 0.06
 LOG_BITMASK      65535
 
 # default PID params for position and velocity-controlled modes
-MAX_POS_XY       0.15
-MAX_POS_YAW      0.3
-MAX_POS_Z        0.15
-MAX_VEL_XY       0.2
-MAX_VEL_YAW      0.4
-MAX_VEL_Z        0.2
+LOIT_MAX_POS_X        0.15
+LOIT_MAX_POS_Y        0.15
+LOIT_MAX_POS_YAW      0.3
+LOIT_MAX_POS_Z        0.15
 
-VELXY_D          1.0
-VELXY_FF         0.0
-VELXY_FLTD       3.0
-VELXY_FLTE       3.0
-VELXY_I          1.5
-VELXY_IMAX       5.0
-VELXY_P          5.0
-VELYAW_D         0.0
-VELYAW_FF        0.0
-VELYAW_FLTD      3.0
-VELYAW_FLTE      3.0
-VELYAW_I         0.8
-VELYAW_IMAX      5.0
-VELYAW_P         10.0
-VELZ_D           0.0
-VELZ_FF          0.0
-VELZ_FLTD        3.0
-VELZ_FLTE        3.0
-VELZ_I           1.0
-VELZ_IMAX        2.0
-VELZ_P           15.0
+LOIT_MAX_VEL_X        0.2
+LOIT_MAX_VEL_Y        0.2
+LOIT_MAX_VEL_YAW      0.4
+LOIT_MAX_VEL_Z        0.2
 
-POSXY_D          0.0
-POSXY_FF         0.0
-POSXY_FLTD       3.0
-POSXY_FLTE       3.0
-POSXY_I          0.05
-POSXY_IMAX       0.1
-POSXY_P          1.0
-POSYAW_D         0.0
-POSYAW_FF        0.0
-POSYAW_FLTD      3.0
-POSYAW_FLTE      3.0
-POSYAW_FLTT      3.0
-POSYAW_I         0.1
-POSYAW_IMAX      1.0
-POSYAW_P         1.0
-POSYAW_SMAX      0.0
-POSZ_D           0.0
-POSZ_FF          0.0
-POSZ_FLTD        3.0
-POSZ_FLTE        3.0
-POSZ_I           0.0
-POSZ_IMAX        0.0
-POSZ_P           0.7
+LOIT_VELX_D          1.0
+LOIT_VELX_FF         0.0
+LOIT_VELX_I          1.5
+LOIT_VELX_IMAX       5.0
+LOIT_VELX_P          5.0
+
+LOIT_VELY_D          1.0
+LOIT_VELY_FF         0.0
+LOIT_VELY_I          1.5
+LOIT_VELY_IMAX       5.0
+LOIT_VELY_P          5.0
+
+LOIT_VELYAW_D         0.0
+LOIT_VELYAW_FF        0.0
+LOIT_VELYAW_I         0.8
+LOIT_VELYAW_IMAX      5.0
+LOIT_VELYAW_P         10.0
+
+LOIT_VELZ_D           0.0
+LOIT_VELZ_FF          0.0
+LOIT_VELZ_I           1.0
+LOIT_VELZ_IMAX        2.0
+LOIT_VELZ_P           15.0
+
+LOIT_POSX_D          0.0
+LOIT_POSX_FF         0.0
+LOIT_POSX_I          0.05
+LOIT_POSX_IMAX       0.1
+LOIT_POSX_P          1.0
+
+LOIT_POSXY_D          0.0
+LOIT_POSXY_FF         0.0
+LOIT_POSXY_I          0.05
+LOIT_POSXY_IMAX       0.1
+LOIT_POSXY_P          1.0
+
+LOIT_POSYAW_D         0.0
+LOIT_POSYAW_FF        0.0
+LOIT_POSYAW_I         0.1
+LOIT_POSYAW_IMAX      1.0
+LOIT_POSYAW_P         1.0
+
+LOIT_POSZ_D           0.0
+LOIT_POSZ_FF          0.0
+LOIT_POSZ_I           0.0
+LOIT_POSZ_IMAX        0.0
+LOIT_POSZ_P           0.7

Tools/autotest/pysim/vehicleinfo.py

@@ -240,12 +240,16 @@ def __init__(self):
         },
     },
     "Blimp": {
-        "default_frame": "Blimp",
+        "default_frame": "blimp",
         "frames": {
-            "Blimp": {
+            "blimp": {
                 "waf_target": "bin/blimp",
                 "default_params_filename": "default_params/blimp.parm",
             },
+            "blimp-motor": {
+                "waf_target": "bin/blimp",
+                "default_params_filename": ["default_params/blimp.parm", "default_params/blimp-motor.parm"],
+            },
         },
     },
     "ArduPlane": {