Merge branch 'ttecg_itx_2'

CMakeLists.txt

@@ -28,6 +28,7 @@ add_message_files(
   Takeoff.msg
   Waypoint.msg
   WeatherData.msg
+  Yaw.msg
 )
 
 add_service_files(

msg/Status.msg

@@ -10,6 +10,8 @@ uint8 MODE_INITIALIZING=8
 uint8 MODE_STABILIZE=9
 uint8 MODE_ALT_HOLD=10
 uint8 MODE_LAND=11
+uint8 MODE_POSHOLD=12
+uint8 MODE_BRAKE=13
 uint8 MODE_UNKNOWN=15
 
 uint8 FENCE_NOT_BREACHED=0

msg/Yaw.msg

+int16 yaw_degrees

src/mavbridge_acs.py

@@ -39,6 +39,8 @@ mode_mav_to_enum = { 'RTL' : apmsg.Status.MODE_RALLY,
                      'STABILIZE' : apmsg.Status.MODE_STABILIZE,
                      'ALT_HOLD' : apmsg.Status.MODE_ALT_HOLD,
                      'LAND' : apmsg.Status.MODE_LAND,
+                     'POSHOLD' : apmsg.Status.MODE_POSHOLD,
+                     'BRAKE' : apmsg.Status.MODE_BRAKE,
                      'INITIALIZING' : apmsg.Status.MODE_INITIALIZING}
 mode_enum_to_mav = { v:k for (k,v) in mode_mav_to_enum.items() }
 
@@ -125,16 +127,10 @@ def pub_status(msg_type, msg, bridge):
     sta.pwr_batt_cur = master.field('SYS_STATUS', 'current_battery', -1)
 
     if (bridge.get_vehicle_type() == 0): #if vehicle type not yet known
-        get_param = bridge.get_module('fpr')._get_param
-        #param FRAME_TYPE is unique to ArduCopter
-        prm = get_param("FRAME_TYPE", tries=1, force=False) 
-        if isinstance(prm, float):
+        if msg.type == mavutil.mavlink.MAV_TYPE_QUADROTOR:
             bridge.set_vehicle_type(enums.AC_COPTER)
-        else:
-            #param RUDDER_ONLY is unique to ArduPlane
-            prm = get_param("RUDDER_ONLY", tries=1, force=False) 
-            if isinstance(prm, float):
-                bridge.set_vehicle_type(enums.AC_FIXED_WING)
+        elif msg.type == mavutil.mavlink.MAV_TYPE_FIXED_WING:
+            bridge.set_vehicle_type(enums.AC_FIXED_WING)
 
     sta.vehicle_type = bridge.get_vehicle_type()
 
@@ -535,6 +531,43 @@ def sub_weather_update(message, bridge):
             int(message.baro_millibars * 10.0), 0, 0, 
             int(message.temp_C * 100.0))
       
+# Purpose: release stores (parameter determines which load to drop)
+# NOTE: for now it's a copy of the SSC-LANT camera control callback
+def sub_release_stores(message, bridge):
+    if message.data == 1:
+        rospy.loginfo("Releasing expendable station 1")
+        for delay in range(1,1000):
+            bridge.get_master().mav.command_long_send(
+                bridge.get_master().target_system,
+                0,
+                mavutil.mavlink.MAV_CMD_DO_SET_SERVO, # command
+                0, # confirmation
+                enums.STORES_RELEASE_CHANNEL, # param1 (channel)
+                1000, # param2 (PWM output value)
+                0, # param3
+                0, # param4
+                0, # param5
+                0, # param6
+                0) # param7
+    elif message.data == 2:
+        rospy.loginfo("Releasing expendable station 2")
+        for delay in range(1,1000):
+            bridge.get_master().mav.command_long_send(
+                bridge.get_master().target_system,
+                0,
+                mavutil.mavlink.MAV_CMD_DO_SET_SERVO, # command
+                0, # confirmation
+                enums.STORES_RELEASE_CHANNEL, # param1 (channel)
+                2000, # param2 (PWM output value)
+                0, # param3
+                0, # param4
+                0, # param5
+                0, # param6
+                0) # param7
+    else:
+        # Invalid ID (for now--may go to 4 at some point)
+        rospy.loginfo("Order to release from invalid expendable station")
+
 """
 sub_copter_takeoff
 Purpose: Send takeoff command to autopilot (required for copters)
@@ -557,6 +590,70 @@ def sub_copter_takeoff(message, bridge):
 
     #TODO: handle takeoff command response from autopilot?
 
+"""
+sub_cam_control
+Purpose: Send camera control command to autopilot, also used to activate a servo in place of camera
+
+"""
+def sub_cam_control(message, bridge):
+    bridge.get_master().mav.command_long_send(
+        bridge.get_master().target_system,
+        0,
+        mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONTROL, # command
+        0, # confirmation
+        0, # param1 (enable)
+        0, # param2
+        0, # param3
+        0, # param4 (focus_lock)
+        1, # param5 (shutter)
+        0, # param6
+        0) # param7
+
+    #TODO: handle command response from autopilot?
+
+"""
+sub_copter_yaw
+Purpose: Send yaw command to autopilot (copter only)
+
+Field:
+yaw_degrees 
+"""
+def sub_copter_yaw(message, bridge):
+    bridge.get_master().mav.command_long_send(
+        bridge.get_master().target_system,
+        bridge.get_master().target_component,
+        mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command
+        0, # confirmation
+        message.yaw_degrees, # param1 (degrees)
+        0, # param2
+        1, # param3 (clockwise (1) / counterclockwise (-1))
+        0, # param4 (absolute (0) / relative (1))
+        0, # param5
+        0, # param6
+        0) # param7
+
+"""
+sub_copter_land
+Purpose: Send land command to autopilot (copter only)
+
+Field:
+lat
+lon 
+"""
+def sub_copter_land(message, bridge):
+    bridge.get_master().mav.command_long_send(
+        bridge.get_master().target_system,
+        bridge.get_master().target_component,
+        mavutil.mavlink.MAV_CMD_NAV_LAND, # command
+        0, # confirmation
+        0, # param1 
+        0, # param2
+        0, # param3 
+        0, # param4 
+        message.land_lat, # param5 (Lat)
+        message.land_lon, # param6 (Lon)
+        0) # param7
+
 #-----------------------------------------------------------------------
 # init()
 
@@ -579,7 +676,10 @@ def init(bridge):
                              sub_payload_waypoint, log=False)
     bridge.add_ros_sub_event("reboot", stdmsg.Empty, sub_reboot)
     bridge.add_ros_sub_event("weather_update", apmsg.WeatherData, sub_weather_update) 
+    bridge.add_ros_sub_event("stores_release", stdmsg.UInt16, sub_release_stores) 
     bridge.add_ros_sub_event("copter_takeoff",apmsg.Takeoff,sub_copter_takeoff)
+    bridge.add_ros_sub_event("cam_control", stdmsg.Empty, sub_cam_control)
+    bridge.add_ros_sub_event("copter_yaw", apmsg.Yaw, sub_copter_yaw)
 
     # Services handled in classes
     acs_cal = Calibrations(bridge)