ACM: added support for MAVLink 1.0 to ArduCopter

most operations should now work

ArduCopter/GCS_Mavlink.pde

@@ -1,5 +1,7 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+#include "Mavlink_compat.h"
+
 // use this to prevent recursion during sensor init
 static bool in_mavlink_delay;
 
@@ -27,10 +29,62 @@ static bool mavlink_active;
 
 static NOINLINE void send_heartbeat(mavlink_channel_t chan)
 {
+#ifdef MAVLINK10
+    uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+    uint8_t system_status = MAV_STATE_ACTIVE;
+    uint32_t custom_mode = control_mode;
+
+    // work out the base_mode. This value is not very useful
+    // for APM, but we calculate it as best we can so a generic
+    // MAVLink enabled ground station can work out something about
+    // what the MAV is up to. The actual bit values are highly
+    // ambiguous for most of the APM flight modes. In practice, you
+    // only get useful information from the custom_mode, which maps to
+    // the APM flight mode and has a well defined meaning in the
+    // ArduPlane documentation
+    base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
+    switch (control_mode) {
+    case AUTO:
+    case RTL:
+    case LOITER:
+    case GUIDED:
+    case CIRCLE:
+        base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
+        // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
+        // APM does in any mode, as that is defined as "system finds its own goal
+        // positions", which APM does not currently do
+        break;
+    }
+
+    // all modes except INITIALISING have some form of manual
+    // override if stick mixing is enabled
+    base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+
+#if HIL_MODE != HIL_MODE_DISABLED
+    base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+#endif
+
+    // we are armed if we are not initialising
+    if (motors.armed()) {
+        base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
+    }
+
+    // indicate we have set a custom mode
+    base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+
     mavlink_msg_heartbeat_send(
         chan,
-        2 /*mavlink_system.type*/,  //MAV_TYPE_QUADROTOR
+        MAV_TYPE_QUADROTOR,
+        MAV_AUTOPILOT_ARDUPILOTMEGA,
+        base_mode,
+        custom_mode,
+        system_status);
+#else // MAVLINK10
+    mavlink_msg_heartbeat_send(
+        chan,
+        MAV_QUADROTOR,
         MAV_AUTOPILOT_ARDUPILOTMEGA);
+#endif // MAVLINK10
 }
 
 static NOINLINE void send_attitude(mavlink_channel_t chan)
@@ -48,6 +102,72 @@ static NOINLINE void send_attitude(mavlink_channel_t chan)
 
 static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops)
 {
+#ifdef MAVLINK10
+    uint32_t control_sensors_present = 0;
+    uint32_t control_sensors_enabled;
+    uint32_t control_sensors_health;
+
+    // first what sensors/controllers we have
+    control_sensors_present |= (1<<0); // 3D gyro present
+    control_sensors_present |= (1<<1); // 3D accelerometer present
+    if (g.compass_enabled) {
+        control_sensors_present |= (1<<2); // compass present
+    }
+    control_sensors_present |= (1<<3); // absolute pressure sensor present
+    if (g_gps != NULL && g_gps->status() == GPS::GPS_OK) {
+        control_sensors_present |= (1<<5); // GPS present
+    }
+    control_sensors_present |= (1<<10); // 3D angular rate control
+    control_sensors_present |= (1<<11); // attitude stabilisation
+    control_sensors_present |= (1<<12); // yaw position
+    control_sensors_present |= (1<<13); // altitude control
+    control_sensors_present |= (1<<14); // X/Y position control
+    control_sensors_present |= (1<<15); // motor control
+
+    // now what sensors/controllers are enabled
+
+    // first the sensors
+    control_sensors_enabled = control_sensors_present & 0x1FF;
+
+    // now the controllers
+    control_sensors_enabled = control_sensors_present & 0x1FF;
+
+    control_sensors_enabled |= (1<<10); // 3D angular rate control
+    control_sensors_enabled |= (1<<11); // attitude stabilisation
+    control_sensors_enabled |= (1<<13); // altitude control
+    control_sensors_enabled |= (1<<15); // motor control
+
+    switch (control_mode) {
+    case AUTO:
+    case RTL:
+    case LOITER:
+    case GUIDED:
+    case CIRCLE:
+    case POSITION:
+        control_sensors_enabled |= (1<<12); // yaw position
+        control_sensors_enabled |= (1<<14); // X/Y position control
+        break;
+    }
+
+    // at the moment all sensors/controllers are assumed healthy
+    control_sensors_health = control_sensors_present;
+
+    uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total1)/(float)g.pack_capacity;	//Mavlink scaling 100% = 1000
+
+    mavlink_msg_sys_status_send(
+        chan,
+        control_sensors_present,
+        control_sensors_enabled,
+        control_sensors_health,
+        0,
+        battery_voltage1 * 1000, // mV
+        0,
+        battery_remaining,      // in %
+        0, // comm drops %,
+        0, // comm drops in pkts,
+        0, 0, 0, 0);
+
+#else // MAVLINK10
     uint8_t mode 	 = MAV_MODE_UNINIT;
     uint8_t nav_mode = MAV_NAV_VECTOR;
 
@@ -89,6 +209,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
         battery_voltage1 * 1000,
         battery_remaining,
         packet_drops);
+#endif // MAVLINK10
 }
 
 static void NOINLINE send_meminfo(mavlink_channel_t chan)
@@ -102,12 +223,15 @@ static void NOINLINE send_location(mavlink_channel_t chan)
     Matrix3f rot = ahrs.get_dcm_matrix(); // neglecting angle of attack for now
     mavlink_msg_global_position_int_send(
         chan,
-        current_loc.lat,
-        current_loc.lng,
-        current_loc.alt * 10,
-        g_gps->ground_speed * rot.a.x,
-        g_gps->ground_speed * rot.b.x,
-        g_gps->ground_speed * rot.c.x);
+        millis(),
+        current_loc.lat,                // in 1E7 degrees
+        current_loc.lng,                // in 1E7 degrees
+        g_gps->altitude*10,             // millimeters above sea level
+        current_loc.alt * 10,           // millimeters above ground
+        g_gps->ground_speed * rot.a.x,  // X speed cm/s
+        g_gps->ground_speed * rot.b.x,  // Y speed cm/s
+        g_gps->ground_speed * rot.c.x,
+        g_gps->ground_course);          // course in 1/100 degree
 }
 
 static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
@@ -162,6 +286,26 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
 
 static void NOINLINE send_gps_raw(mavlink_channel_t chan)
 {
+#ifdef MAVLINK10
+    uint8_t fix = g_gps->status();
+    if (fix == GPS::GPS_OK) {
+        fix = 3;
+    }
+
+    mavlink_msg_gps_raw_int_send(
+        chan,
+        micros(),
+        fix,
+        g_gps->latitude,      // in 1E7 degrees
+        g_gps->longitude,     // in 1E7 degrees
+        g_gps->altitude * 10, // in mm
+        g_gps->hdop,
+        65535,
+        g_gps->ground_speed,  // cm/s
+        g_gps->ground_course, // 1/100 degrees,
+        g_gps->num_sats);
+
+#else // MAVLINK10
     mavlink_msg_gps_raw_send(
         chan,
         micros(),
@@ -173,6 +317,7 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan)
         current_loc.alt / 100.0, // was 0
         g_gps->ground_speed / 100.0,
         g_gps->ground_course / 100.0);
+#endif  // MAVLINK10
 }
 
 static void NOINLINE send_servo_out(mavlink_channel_t chan)
@@ -185,6 +330,8 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
 
         mavlink_msg_rc_channels_scaled_send(
         chan,
+        millis(),
+        0, // port 0
         g.rc_1.servo_out,
         g.rc_2.servo_out,
         g.rc_3.radio_out,
@@ -200,6 +347,8 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
 			if(motors.armed() && motors.auto_armed()){
 				mavlink_msg_rc_channels_scaled_send(
 					chan,
+                    millis(),
+                    0, // port 0
 					g.rc_1.servo_out,
 					g.rc_2.servo_out,
 					10000 * g.rc_3.norm_output(),
@@ -212,6 +361,8 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
 		   }else{
 				mavlink_msg_rc_channels_scaled_send(
 					chan,
+                    millis(),
+                    0, // port 0
 					0,
 					0,
 					-10000,
@@ -226,6 +377,8 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
 		#else
 			mavlink_msg_rc_channels_scaled_send(
 				chan,
+                millis(),
+                0, // port 0
 				g.rc_1.servo_out,
 				g.rc_2.servo_out,
 				g.rc_3.radio_out,
@@ -244,6 +397,8 @@ static void NOINLINE send_radio_in(mavlink_channel_t chan)
     const uint8_t rssi = 1;
     mavlink_msg_rc_channels_raw_send(
         chan,
+        millis(),
+        0, // port
         g.rc_1.radio_in,
         g.rc_2.radio_in,
         g.rc_3.radio_in,
@@ -259,6 +414,8 @@ static void NOINLINE send_radio_out(mavlink_channel_t chan)
 {
     mavlink_msg_servo_output_raw_send(
         chan,
+        micros(),
+        0, // port
         motors.motor_out[AP_MOTORS_MOT_1],
         motors.motor_out[AP_MOTORS_MOT_2],
         motors.motor_out[AP_MOTORS_MOT_3],
@@ -397,7 +554,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
         break;
 
     case MSG_GPS_RAW:
+#ifdef MAVLINK10
+        CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
+#else
         CHECK_PAYLOAD_SIZE(GPS_RAW);
+#endif
         send_gps_raw(chan);
         break;
 
@@ -576,7 +737,7 @@ void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char
 	mavlink_msg_statustext_send(
 				chan,
 				severity,
-				(const int8_t*) str);
+				str);
 }
 }
 
@@ -889,6 +1050,67 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			break;
 		}
 
+#ifdef MAVLINK10
+    case MAVLINK_MSG_ID_COMMAND_LONG:
+        {
+            // decode
+            mavlink_command_long_t packet;
+            mavlink_msg_command_long_decode(msg, &packet);
+            if (mavlink_check_target(packet.target_system, packet.target_component)) break;
+
+            uint8_t result;
+
+            // do command
+            send_text(SEVERITY_LOW,PSTR("command received: "));
+
+            switch(packet.command) {
+
+            case MAV_CMD_NAV_LOITER_UNLIM:
+                set_mode(LOITER);
+                result = MAV_RESULT_ACCEPTED;
+                break;
+
+            case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+                set_mode(RTL);
+                result = MAV_RESULT_ACCEPTED;
+                break;
+
+            case MAV_CMD_NAV_LAND:
+                set_mode(LAND);
+                result = MAV_RESULT_ACCEPTED;
+                break;
+
+            case MAV_CMD_MISSION_START:
+                set_mode(AUTO);
+                result = MAV_RESULT_ACCEPTED;
+                break;
+
+            case MAV_CMD_PREFLIGHT_CALIBRATION:
+                if (packet.param1 == 1 ||
+                    packet.param2 == 1 ||
+                    packet.param3 == 1) {
+					imu.init_accel(mavlink_delay, flash_leds);
+                }
+                if (packet.param4 == 1) {
+                    trim_radio();
+                }
+                result = MAV_RESULT_ACCEPTED;
+                break;
+
+
+            default:
+                result = MAV_RESULT_UNSUPPORTED;
+                break;
+            }
+
+            mavlink_msg_command_ack_send(
+                chan,
+                packet.command,
+                result);
+
+            break;
+        }
+#else // !MAVLINK10
 	case MAVLINK_MSG_ID_ACTION: //10
 		{
 			// decode
@@ -1021,6 +1243,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
 			break;
 		}
+#endif // MAVLINK10
 
 	case MAVLINK_MSG_ID_SET_MODE:  //11
 		{
@@ -1028,7 +1251,32 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			mavlink_set_mode_t packet;
 			mavlink_msg_set_mode_decode(msg, &packet);
 
-			switch(packet.mode){
+#ifdef MAVLINK10
+            if (!(packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) {
+                // we ignore base_mode as there is no sane way to map
+                // from that bitmap to a APM flight mode. We rely on
+                // custom_mode instead.
+                break;
+            }
+            switch (packet.custom_mode) {
+            case STABILIZE:
+            case ACRO:
+            case ALT_HOLD:
+            case AUTO:
+            case GUIDED:
+            case LOITER:
+            case RTL:
+            case CIRCLE:
+            case POSITION:
+            case LAND:
+            case OF_LOITER:
+            case APPROACH:
+                set_mode(packet.custom_mode);
+                break;
+            }
+
+#else // MAVLINK10
+			switch (packet.mode) {
 
 				case MAV_MODE_MANUAL:
 					set_mode(STABILIZE);
@@ -1049,6 +1297,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 					set_mode(STABILIZE);
 					break;
 			}
+#endif // MAVLINK10
+            break;
 		}
 
 	/*case MAVLINK_MSG_ID_SET_NAV_MODE:
@@ -1541,8 +1791,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 				// a fractional value to an integer type
 				mavlink_msg_param_value_send(
 					chan,
-					(int8_t *)key,
+					key,
                     vp->cast_to_float(var_type),
+                    mav_var_type(var_type),
 					_count_parameters(),
 					-1); // XXX we don't actually know what its index is...
 
@@ -1780,8 +2031,9 @@ GCS_MAVLINK::queued_param_send()
 
     mavlink_msg_param_value_send(
         chan,
-        (int8_t*)param_name,
+        param_name,
         value,
+        mav_var_type(_queued_parameter_type),
         _queued_parameter_count,
         _queued_parameter_index);
 

ArduCopter/Makefile

@@ -33,6 +33,9 @@ mavlink10:
 sitl:
 	make -f ../libraries/Desktop/Makefile.desktop
 
+sitl-mavlink10:
+	make -f ../libraries/Desktop/Makefile.desktop EXTRAFLAGS="-DMAVLINK10"
+
 sitl-octa:
 	make -f ../libraries/Desktop/Makefile.desktop octa