From 712b7184be041c063d14fc7eac7115b8c582e112 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 24 Apr 2012 19:53:24 +1000 Subject: [PATCH] ACM: added support for MAVLink 1.0 to ArduCopter most operations should now work --- ArduCopter/GCS_Mavlink.pde | 274 +++++++++++++++++++++++++++++++++++-- ArduCopter/Makefile | 3 + 2 files changed, 266 insertions(+), 11 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index f6692591d9..8732be5839 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/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); diff --git a/ArduCopter/Makefile b/ArduCopter/Makefile index 39784e4912..32e2e44a17 100644 --- a/ArduCopter/Makefile +++ b/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