autotest: added mavlink gimbal simulation in copter SITL

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>

Tools/autotest/pysim/gimbal.py

@@ -0,0 +1,223 @@
+#!/usr/bin/env python
+
+from aircraft import Aircraft
+import util, time, math
+from math import degrees, radians
+from rotmat import Vector3, Matrix3
+
+def constrain(value, minv, maxv):
+    '''constrain a value to a range'''
+    if value < minv:
+        value = minv
+    if value > maxv:
+        value = maxv
+    return value
+
+class Gimbal3Axis(object):
+    '''a gimbal simulation'''
+    def __init__(self, vehicle):
+        # vehicle is the vehicle (usually a multicopter) that the gimbal is attached to
+        self.vehicle = vehicle
+
+        # URL of SITL 2nd telem port
+        self.mavlink_url = 'tcp:127.0.0.1:5762'
+
+        # rotation matrix (gimbal body -> earth)
+        self.dcm = None
+
+        # time of last update
+        self.last_update_t = time.time()
+
+        # true angular rate of gimbal in body frame (rad/s)
+        self.gimbal_angular_rate = Vector3()
+
+        # observed angular rate (including biases)
+        self.gyro = Vector3()
+
+        # joint angles, in radians. in yaw/roll/pitch order. Relative to fwd.
+        # So 0,0,0 points forward.
+        # Pi/2,0,0 means pointing right
+        # 0, Pi/2, 0 means pointing fwd, but rolled 90 degrees to right
+        # 0, 0, -Pi/2, means pointing down
+        self.joint_angles = Vector3()
+
+        # physical constraints on joint angles
+        self.min_yaw = radians(-7.5)
+        self.max_yaw = radians(7.5)
+        self.min_roll = radians(-40)
+        self.max_roll = radians(40)
+        self.min_pitch = radians(-135)
+        self.max_pitch = radians(45)
+
+        # true gyro bias
+        self.true_gyro_bias = Vector3()
+
+        ##################################
+        # reporting variables. gimbal pushes these to vehicle code over MAVLink at approx 100Hz
+
+        # reporting rate in Hz
+        self.reporting_rate = 100
+        
+        # integral of gyro vector over last time interval. In radians
+        self.delta_angle = Vector3()
+
+        # integral of accel vector over last time interval. In m/s
+        self.delta_velocity = Vector3()
+        # we also push joint_angles
+
+        ##################################
+        # control variables from the vehicle
+
+        # angular rate in rad/s. In body frame of gimbal
+        self.demanded_angular_rate = Vector3()
+
+        # gyro bias provided by EKF on vehicle. In rad/s.
+        # Should be subtracted from the gyro readings to get true body rotatation rates
+        self.supplied_gyro_bias = Vector3()
+
+        ###################################
+        # communication variables
+        self.connection = None
+        self.counter = 0
+        self.last_report_t = time.time()
+        self.last_heartbeat_t = time.time()
+        self.seen_heartbeat = False
+        self.seen_gimbal_control = False
+
+    def update(self):
+        '''update the gimbal state'''
+
+        # calculate delta time in seconds
+        now = time.time()
+        delta_t = now - self.last_update_t
+        self.last_update_t = now
+
+        # for the moment we will set gyros equal to demanded_angular_rate
+        self.gimbal_angular_rate = self.demanded_angular_rate + self.true_gyro_bias - self.supplied_gyro_bias
+
+        if self.dcm is None:
+            # start with copter rotation matrix
+            self.dcm = self.vehicle.dcm.copy()
+
+        # update rotation of the gimbal
+        self.dcm.rotate(self.gimbal_angular_rate*delta_t)
+        self.dcm.normalize()
+
+        # calculate copter -> gimbal rotation matrix
+        rotmat_copter_gimbal = self.dcm.transposed() * self.vehicle.dcm
+
+        # calculate joint angles (euler312 order)
+        self.joint_angles = Vector3(*rotmat_copter_gimbal.to_euler312())
+
+        # constrain joint angles
+        need_new_dcm = False
+
+        # constrain observed gyro (prevent observed rotation past gimbal limits)
+        # NOTE: this needs to be replaced later with code that first converts rates
+        # from 312 rates to body rates and back again
+        if self.joint_angles.x >= self.max_yaw:
+            need_new_dcm = True
+            self.joint_angles.x = self.max_yaw
+            if self.gimbal_angular_rate.z > 0:
+                self.gimbal_angular_rate.z = 0
+        if self.joint_angles.x <= self.min_yaw:
+            need_new_dcm = True
+            self.joint_angles.x = self.min_yaw
+            if self.gimbal_angular_rate.z < 0:
+                self.gimbal_angular_rate.z = 0
+        if self.joint_angles.y >= self.max_roll:
+            need_new_dcm = True
+            self.joint_angles.y = self.max_roll
+            if self.gimbal_angular_rate.x > 0:
+                self.gimbal_angular_rate.x = 0
+        if self.joint_angles.y <= self.min_roll:
+            need_new_dcm = True
+            self.joint_angles.y = self.min_roll
+            if self.gimbal_angular_rate.x < 0:
+                self.gimbal_angular_rate.x = 0
+        if self.joint_angles.z >= self.max_pitch:
+            need_new_dcm = True
+            self.joint_angles.z = self.max_pitch
+            if self.gimbal_angular_rate.y > 0:
+                self.gimbal_angular_rate.y = 0
+        if self.joint_angles.z <= self.min_pitch:
+            need_new_dcm = True
+            self.joint_angles.z = self.min_pitch
+            if self.gimbal_angular_rate.y < 0:
+                self.gimbal_angular_rate.y = 0
+
+        if need_new_dcm:
+            # when we hit the gimbal limits we need to recalc the matrix
+            rotmat_copter_gimbal.from_euler312(self.joint_angles.x, self.joint_angles.y, self.joint_angles.z)
+            self.dcm = self.vehicle.dcm * rotmat_copter_gimbal.transposed()
+
+        # update observed gyro
+        self.gyro = self.gimbal_angular_rate + self.true_gyro_bias
+
+        # update delta_angle (integrate)
+        self.delta_angle += self.gyro * delta_t
+
+        # calculate accel in gimbal body frame
+        copter_accel_earth = self.vehicle.dcm * self.vehicle.accel_body
+        accel = self.dcm.transposed() * copter_accel_earth
+
+        # integrate velocity
+        self.delta_velocity += accel * delta_t
+
+        # see if we should do a report
+        if now - self.last_report_t >= 1.0 / self.reporting_rate:
+            self.send_report()
+            self.last_report_t = now
+
+    def send_report(self):
+        '''send a report to the vehicle control code over MAVLink'''
+        from pymavlink import mavutil
+        if self.connection is None:
+            try:
+                self.connection = mavutil.mavlink_connection(self.mavlink_url, retries=0)
+            except Exception as e:
+                return
+            print("Gimbal connected to %s" % self.mavlink_url)
+            
+        # check for incoming control messages
+        while True:
+            m = self.connection.recv_match(type=['GIMBAL_CONTROL','HEARTBEAT'], blocking=False)
+            if m is None:
+                break
+            if m.get_type() == 'GIMBAL_CONTROL':
+                self.demanded_angular_rate = Vector3(m.demanded_rate_x,
+                                                     m.demanded_rate_y,
+                                                     m.demanded_rate_z)
+                self.supplied_gyro_bias = Vector3(m.gyro_bias_x,
+                                                  m.gyro_bias_y,
+                                                  m.gyro_bias_z)
+                self.seen_gimbal_control = True
+                print("rate=%s" % self.demanded_angular_rate)
+            if m.get_type() == 'HEARTBEAT' and not self.seen_heartbeat:
+                self.seen_heartbeat = True
+                self.connection.mav.srcSystem = m.get_srcSystem()
+                self.connection.mav.srcComponent = mavutil.mavlink.MAV_COMP_ID_GIMBAL
+                print("Gimbal using srcSystem %u" % self.connection.mav.srcSystem)
+
+        if self.seen_heartbeat:
+            now = time.time()
+            if now - self.last_heartbeat_t >= 1:
+                self.connection.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GIMBAL,
+                                                   mavutil.mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA,
+                                                   0, 0, 0)
+                self.last_heartbeat_t = now
+
+        if self.seen_heartbeat:
+            self.connection.mav.gimbal_report_send(self.counter,
+                                                   self.delta_angle.x,
+                                                   self.delta_angle.y,
+                                                   self.delta_angle.z,
+                                                   self.delta_velocity.x,
+                                                   self.delta_velocity.y,
+                                                   self.delta_velocity.z,
+                                                   self.joint_angles.x,
+                                                   self.joint_angles.y,
+                                                   self.joint_angles.z)
+            self.delta_velocity.zero()
+            self.delta_angle.zero()
+            self.counter += 1

Tools/autotest/pysim/sim_multicopter.py

@@ -96,6 +96,7 @@ parser.add_option("--home", dest="home",  type='string', default=None, help="hom
 parser.add_option("--rate", dest="rate", type='int', help="SIM update rate", default=400)
 parser.add_option("--wind", dest="wind", help="Simulate wind (speed,direction,turbulance)", default='0,0,0')
 parser.add_option("--frame", dest="frame", help="frame type (+,X,octo)", default='+')
+parser.add_option("--gimbal", dest="gimbal", action='store_true', default=False, help="enable gimbal")
 
 (opts, args) = parser.parse_args()
 
@@ -165,6 +166,13 @@ print("Starting at lat=%f lon=%f alt=%.1f heading=%.1f" % (
 frame_time = 1.0/opts.rate
 sleep_overhead = 0
 
+if opts.gimbal:
+    from gimbal import Gimbal3Axis
+    gimbal = Gimbal3Axis(a)
+    print("Adding gimbal support")
+else:
+    gimbal = None
+
 while True:
     frame_start = time.time()
     sim_recv(m)
@@ -172,6 +180,8 @@ while True:
     m2 = m[:]
 
     a.update(m2)
+    if gimbal is not None:
+        gimbal.update()
     sim_send(m, a)
     frame_count += 1
     t = time.time()

Tools/autotest/sim_vehicle.sh

@@ -13,6 +13,7 @@ INSTANCE=0
 USE_VALGRIND=0
 USE_GDB=0
 USE_GDB_STOPPED=0
+USE_MAVLINK_GIMBAL=0
 CLEAN_BUILD=0
 START_ANTENNA_TRACKER=0
 WIPE_EEPROM=0
@@ -63,7 +64,7 @@ EOF
 
 
 # parse options. Thanks to http://wiki.bash-hackers.org/howto/getopts_tutorial
-while getopts ":I:VgGcj:TA:t:L:v:hwf:RNHe" opt; do
+while getopts ":I:VgGcj:TA:t:L:v:hwf:RNHeM" opt; do
   case $opt in
     v)
       VEHICLE=$OPTARG
@@ -93,6 +94,9 @@ while getopts ":I:VgGcj:TA:t:L:v:hwf:RNHe" opt; do
     G)
       USE_GDB=1
       ;;
+    M)
+      USE_MAVLINK_GIMBAL=1
+      ;;
     g)
       USE_GDB=1
       USE_GDB_STOPPED=1
@@ -203,6 +207,11 @@ case $FRAME in
         ;;
 esac
 
+if [ $USE_MAVLINK_GIMBAL == 1 ]; then
+    echo "Using MAVLink gimbal"
+    EXTRA_SIM="--gimbal"
+fi
+
 autotest=$(dirname $(readlink -e $0))
 if [ $NO_REBUILD == 0 ]; then
 pushd $autotest/../../$VEHICLE || {
@@ -343,5 +352,8 @@ fi
 if [ $START_HIL == 1 ]; then
     options="$options --load-module=HIL"
 fi
+if [ $USE_MAVLINK_GIMBAL == 1 ]; then
+    options="$options --load-module=gimbal"
+fi
 mavproxy.py $options --cmd="$extra_cmd" $*
 kill_tasks