Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

MAVLink: remove MAVLink 0.9 protocol support 

this simplifies the code a lot. We're not going back to 1.0
This commit is contained in:
Andrew Tridgell 2012-07-05 10:55:58 +10:00
parent adf6030cfa
commit 60caaa4b04
16 changed files with 10 additions and 1064 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

285
APMrover2/GCS_Mavlink.pde
View File

@ -27,7 +27,6 @@ static bool mavlink_active;
static NOINLINE void send_heartbeat(mavlink_channel_t chan)
{
#if MAVLINK10 == ENABLED
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t system_status = MAV_STATE_ACTIVE;
uint32_t custom_mode = control_mode;
@ -92,12 +91,6 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
base_mode,
custom_mode,
system_status);
#else // MAVLINK10
mavlink_msg_heartbeat_send(
chan,
mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA);
#endif // MAVLINK10
}
static NOINLINE void send_attitude(mavlink_channel_t chan)
@ -125,7 +118,6 @@ static NOINLINE void send_fence_status(mavlink_channel_t chan)
static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops)
{
#if MAVLINK10 == ENABLED
uint32_t control_sensors_present = 0;
uint32_t control_sensors_enabled;
uint32_t control_sensors_health;
@ -228,69 +220,6 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
0, // comm drops in pkts,
0, 0, 0, 0);
#else // MAVLINK10
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
switch(control_mode) {
case MANUAL:
mode = MAV_MODE_MANUAL;
break;
case LEARNING:
mode = MAV_MODE_TEST1;
break;
case FLY_BY_WIRE_A:
mode = MAV_MODE_TEST2;
nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc.
break;
case FLY_BY_WIRE_B:
mode = MAV_MODE_TEST2;
nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc.
break;
case GUIDED:
mode = MAV_MODE_GUIDED;
break;
case AUTO:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
break;
case RTL:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
case LOITER:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LOITER;
break;
case INITIALISING:
mode = MAV_MODE_UNINIT;
nav_mode = MAV_NAV_GROUNDED;
break;
case CIRCLE:
mode = MAV_MODE_TEST3;
break;
}
uint8_t status = MAV_STATE_ACTIVE;
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total1)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
if (g.battery_monitoring == 3) {
/*setting a out-of-range value.
It informs to external devices that
it cannot be calculated properly just by voltage*/
battery_remaining = 1500;
}
mavlink_msg_sys_status_send(
chan,
mode,
nav_mode,
status,
load * 1000,
battery_voltage1 * 1000,
battery_remaining,
packet_drops);
#endif // MAVLINK10
}
static void NOINLINE send_meminfo(mavlink_channel_t chan)
@ -332,7 +261,6 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
{
#if MAVLINK10 == ENABLED
uint8_t fix = g_gps->status();
if (fix == GPS::GPS_OK) {
fix = 3;
@ -350,20 +278,6 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan)
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(),
g_gps->status(),
g_gps->latitude / 1.0e7,
g_gps->longitude / 1.0e7,
g_gps->altitude / 100.0,
g_gps->hdop,
0.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)
@ -621,11 +535,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
break;
case MSG_GPS_RAW:
#if MAVLINK10 == ENABLED
CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
#else
CHECK_PAYLOAD_SIZE(GPS_RAW);
#endif
send_gps_raw(chan);
break;
@ -1111,7 +1021,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#if MAVLINK10 == ENABLED
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// decode
@ -1169,135 +1078,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#else // MAVLINK10
case MAVLINK_MSG_ID_ACTION:
{
// decode
mavlink_action_t packet;
mavlink_msg_action_decode(msg, &packet);
if (mavlink_check_target(packet.target,packet.target_component)) break;
uint8_t result = 0;
// do action
send_text(SEVERITY_LOW,PSTR("action received: "));
//Serial.println(packet.action);
switch(packet.action){
case MAV_ACTION_LAUNCH:
//set_mode(TAKEOFF);
break;
case MAV_ACTION_RETURN:
set_mode(RTL);
result=1;
break;
case MAV_ACTION_EMCY_LAND:
//set_mode(LAND);
break;
case MAV_ACTION_HALT:
do_loiter_at_location();
result=1;
break;
/* No mappable implementation in APM 2.0
case MAV_ACTION_MOTORS_START:
case MAV_ACTION_CONFIRM_KILL:
case MAV_ACTION_EMCY_KILL:
case MAV_ACTION_MOTORS_STOP:
case MAV_ACTION_SHUTDOWN:
break;
*/
case MAV_ACTION_CONTINUE:
process_next_command();
result=1;
break;
case MAV_ACTION_SET_MANUAL:
set_mode(MANUAL);
result=1;
break;
case MAV_ACTION_SET_AUTO:
set_mode(AUTO);
result=1;
// clearing failsafe should not be needed
// here. Added based on some puzzling results in
// the simulator (tridge)
failsafe = FAILSAFE_NONE;
break;
case MAV_ACTION_STORAGE_READ:
// we load all variables at startup, and
// save on each mavlink set
result=1;
break;
case MAV_ACTION_STORAGE_WRITE:
// this doesn't make any sense, as we save
// all settings as they come in
result=1;
break;
case MAV_ACTION_CALIBRATE_RC: break;
trim_radio();
result=1;
break;
case MAV_ACTION_CALIBRATE_GYRO:
case MAV_ACTION_CALIBRATE_MAG:
case MAV_ACTION_CALIBRATE_ACC:
case MAV_ACTION_CALIBRATE_PRESSURE:
case MAV_ACTION_REBOOT: // this is a rough interpretation
#if LITE == DISABLED
startup_IMU_ground(true);
#endif
result=1;
break;
/* For future implemtation
case MAV_ACTION_REC_START: break;
case MAV_ACTION_REC_PAUSE: break;
case MAV_ACTION_REC_STOP: break;
*/
/* Takeoff is not an implemented flight mode in APM 2.0
case MAV_ACTION_TAKEOFF:
set_mode(TAKEOFF);
break;
*/
case MAV_ACTION_NAVIGATE:
set_mode(AUTO);
result=1;
break;
/* Land is not an implemented flight mode in APM 2.0
case MAV_ACTION_LAND:
set_mode(LAND);
break;
*/
case MAV_ACTION_LOITER:
set_mode(LOITER);
result=1;
break;
default: break;
}
mavlink_msg_action_ack_send(
chan,
packet.action,
result
);
break;
}
#endif
case MAVLINK_MSG_ID_SET_MODE:
{
@ -1305,7 +1085,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_set_mode_t packet;
mavlink_msg_set_mode_decode(msg, &packet);
#if MAVLINK10 == ENABLED
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
@ -1326,54 +1105,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#else // MAVLINK10
switch(packet.mode){
case MAV_MODE_MANUAL:
set_mode(MANUAL);
break;
case MAV_MODE_GUIDED:
set_mode(GUIDED);
break;
case MAV_MODE_AUTO:
if(mav_nav == 255 || mav_nav == MAV_NAV_WAYPOINT) set_mode(AUTO);
if(mav_nav == MAV_NAV_RETURNING) set_mode(RTL);
if(mav_nav == MAV_NAV_LOITER) set_mode(LOITER);
mav_nav = 255;
break;
case MAV_MODE_TEST1:
set_mode(LEARNING);
break;
case MAV_MODE_TEST2:
if(mav_nav == 255 || mav_nav == 1) set_mode(FLY_BY_WIRE_A);
if(mav_nav == 2) set_mode(FLY_BY_WIRE_B);
//if(mav_nav == 3) set_mode(FLY_BY_WIRE_C);
mav_nav = 255;
break;
}
#endif
break;
}
#if MAVLINK10 == DISABLED
case MAVLINK_MSG_ID_SET_NAV_MODE:
{
// decode
mavlink_set_nav_mode_t packet;
mavlink_msg_set_nav_mode_decode(msg, &packet);
// To set some flight modes we must first receive a "set nav mode" message and then a "set mode" message
mav_nav = packet.nav_mode;
break;
}
#endif // MAVLINK10
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
{
// decode
@ -1713,9 +1447,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
default:
#if MAVLINK10 == ENABLED
result = MAV_MISSION_UNSUPPORTED;
#endif
break;
}
@ -1929,7 +1661,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
#if HIL_MODE != HIL_MODE_DISABLED
// This is used both as a sensor and to pass the location
// in HIL_ATTITUDE mode.
#if MAVLINK10 == ENABLED
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
// decode
@ -1942,19 +1673,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
packet.vel*1.0e-2, packet.cog*1.0e-2, 0, 0);
break;
}
#else // MAVLINK10
case MAVLINK_MSG_ID_GPS_RAW:
{
// decode
mavlink_gps_raw_t packet;
mavlink_msg_gps_raw_decode(msg, &packet);
// set gps hil sensor
g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt,
packet.v,packet.hdg,0,0);
break;
}
#endif // MAVLINK10
// Is this resolved? - MAVLink protocol change.....
case MAVLINK_MSG_ID_VFR_HUD:
@ -1967,7 +1685,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
airspeed = 100*packet.airspeed;
break;
}
#if MAVLINK10 == ENABLED
case MAVLINK_MSG_ID_HIL_STATE:
{
mavlink_hil_state_t packet;
@ -2008,7 +1726,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#endif // MAVLINK10
#endif
#if HIL_MODE == HIL_MODE_ATTITUDE
case MAVLINK_MSG_ID_ATTITUDE:

3
APMrover2/Makefile
View File

@ -24,9 +24,6 @@ apm2:
apm2beta:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DAPM2_BETA_HARDWARE"
mavlink10:
make -f Makefile EXTRAFLAGS="-DMAVLINK10"
sitl:
make -f ../libraries/Desktop/Makefile.desktop

4
ArduCopter/APM_Config.h
View File

@ -109,7 +109,3 @@
// #define MOT_6 CH_4
// #define MOT_7 CH_7
// #define MOT_8 CH_8
// use this to enable the new MAVLink 1.0 protocol, instead of the
// older 0.9 protocol
// #define MAVLINK10 ENABLED

305
ArduCopter/GCS_Mavlink.pde
View File

@ -29,7 +29,6 @@ static bool mavlink_active;
static NOINLINE void send_heartbeat(mavlink_channel_t chan)
{
#if MAVLINK10 == ENABLED
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t system_status = MAV_STATE_ACTIVE;
uint32_t custom_mode = control_mode;
@ -79,12 +78,6 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
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)
@ -102,7 +95,6 @@ static NOINLINE void send_attitude(mavlink_channel_t chan)
static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops)
{
#if MAVLINK10 == ENABLED
uint32_t control_sensors_present = 0;
uint32_t control_sensors_enabled;
uint32_t control_sensors_health;
@ -182,49 +174,6 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
0, // comm drops in pkts,
0, 0, 0, 0);
#else // MAVLINK10
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
switch(control_mode) {
case LOITER:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_HOLD;
break;
case AUTO:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
break;
case RTL:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
case GUIDED:
mode = MAV_MODE_GUIDED;
nav_mode = MAV_NAV_WAYPOINT;
break;
default:
mode = control_mode + 100;
}
uint8_t status = MAV_STATE_ACTIVE;
if (!motors.armed()) {
status = MAV_STATE_STANDBY;
}
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,
mode,
nav_mode,
status,
0,
battery_voltage1 * 1000,
battery_remaining,
packet_drops);
#endif // MAVLINK10
}
static void NOINLINE send_meminfo(mavlink_channel_t chan)
@ -300,7 +249,6 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
{
#if MAVLINK10 == ENABLED
uint8_t fix = g_gps->status();
if (fix == GPS::GPS_OK) {
fix = 3;
@ -319,19 +267,6 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan)
g_gps->ground_course, // 1/100 degrees,
g_gps->num_sats);
#else // MAVLINK10
mavlink_msg_gps_raw_send(
chan,
g_gps->last_fix_time*(uint64_t)1000,
g_gps->status(),
g_gps->latitude / 1.0e7,
g_gps->longitude / 1.0e7,
g_gps->altitude / 100.0,
g_gps->hdop,
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)
@ -568,11 +503,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
break;
case MSG_GPS_RAW:
#if MAVLINK10 == ENABLED
CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
#else
CHECK_PAYLOAD_SIZE(GPS_RAW);
#endif
send_gps_raw(chan);
break;
@ -990,12 +921,6 @@ GCS_MAVLINK::send_text(gcs_severity severity, const prog_char_t *str)
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
struct Location tell_command = {}; // command for telemetry
#if MAVLINK10 == ENABLED
#else
static uint8_t mav_nav = 255; // For setting mode (some require receipt of 2 messages...)
#endif
switch (msg->msgid) {
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: //66
@ -1073,7 +998,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#if MAVLINK10 == ENABLED
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// decode
@ -1133,140 +1057,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#else // !MAVLINK10
case MAVLINK_MSG_ID_ACTION: //10
{
// decode
mavlink_action_t packet;
mavlink_msg_action_decode(msg, &packet);
if (mavlink_check_target(packet.target,packet.target_component)) break;
if (in_mavlink_delay) {
// don't execute action commands while in sensor
// initialisation
break;
}
uint8_t result = 0;
// do action
send_text(SEVERITY_LOW,PSTR("action received: "));
//Serial.println(packet.action);
switch(packet.action){
case MAV_ACTION_LAUNCH:
//set_mode(TAKEOFF);
break;
case MAV_ACTION_RETURN:
set_mode(RTL);
result=1;
break;
case MAV_ACTION_EMCY_LAND:
set_mode(LAND);
break;
case MAV_ACTION_HALT:
do_loiter_at_location();
result=1;
break;
/* No mappable implementation in APM 2.0
case MAV_ACTION_MOTORS_START:
case MAV_ACTION_CONFIRM_KILL:
case MAV_ACTION_EMCY_KILL:
case MAV_ACTION_MOTORS_STOP:
case MAV_ACTION_SHUTDOWN:
break;
*/
case MAV_ACTION_CONTINUE:
//process_command_queue();
result=1;
break;
case MAV_ACTION_SET_MANUAL:
set_mode(STABILIZE);
result=1;
break;
case MAV_ACTION_SET_AUTO:
set_mode(AUTO);
result=1;
break;
case MAV_ACTION_STORAGE_READ:
// we load all variables at startup, and
// save on each mavlink set
result=1;
break;
case MAV_ACTION_STORAGE_WRITE:
// this doesn't make any sense, as we save
// all settings as they come in
result=1;
break;
case MAV_ACTION_CALIBRATE_RC: break;
trim_radio();
result=1;
break;
case MAV_ACTION_CALIBRATE_GYRO:
case MAV_ACTION_CALIBRATE_MAG:
case MAV_ACTION_CALIBRATE_PRESSURE:
break;
case MAV_ACTION_CALIBRATE_ACC:
imu.init_accel(mavlink_delay, flash_leds);
result=1;
break;
//case MAV_ACTION_REBOOT: // this is a rough interpretation
//startup_IMU_ground();
//result=1;
// break;
/* For future implemtation
case MAV_ACTION_REC_START: break;
case MAV_ACTION_REC_PAUSE: break;
case MAV_ACTION_REC_STOP: break;
*/
/* Takeoff is not an implemented flight mode in APM 2.0
case MAV_ACTION_TAKEOFF:
set_mode(TAKEOFF);
break;
*/
case MAV_ACTION_NAVIGATE:
set_mode(AUTO);
result=1;
break;
case MAV_ACTION_LAND:
set_mode(LAND);
break;
case MAV_ACTION_LOITER:
set_mode(LOITER);
result=1;
break;
default: break;
}
mavlink_msg_action_ack_send(
chan,
packet.action,
result
);
break;
}
#endif // MAVLINK10
case MAVLINK_MSG_ID_SET_MODE: //11
{
@ -1274,7 +1064,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_set_mode_t packet;
mavlink_msg_set_mode_decode(msg, &packet);
#if MAVLINK10 == ENABLED
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
@ -1297,29 +1086,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#else // MAVLINK.9
switch (packet.mode) {
case MAV_MODE_MANUAL:
set_mode(STABILIZE);
break;
case MAV_MODE_GUIDED:
set_mode(GUIDED);
break;
case MAV_MODE_AUTO:
if(mav_nav == 255 || mav_nav == MAV_NAV_WAYPOINT) set_mode(AUTO);
if(mav_nav == MAV_NAV_RETURNING) set_mode(RTL);
if(mav_nav == MAV_NAV_LOITER) set_mode(LOITER);
mav_nav = 255;
break;
case MAV_MODE_TEST1:
set_mode(STABILIZE);
break;
}
#endif
break;
}
@ -1849,75 +1615,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#if HIL_MODE != HIL_MODE_DISABLED && MAVLINK10 != ENABLED
// This is used both as a sensor and to pass the location
// in HIL_ATTITUDE mode. Note that MAVLINK10 uses HIL_STATE
// instead
case MAVLINK_MSG_ID_GPS_RAW: //32
{
// decode
mavlink_gps_raw_t packet;
mavlink_msg_gps_raw_decode(msg, &packet);
// set gps hil sensor
g_gps->setHIL(packet.usec/1000,packet.lat,packet.lon,packet.alt,
packet.v,packet.hdg,0,0);
if (gps_base_alt == 0) {
gps_base_alt = packet.alt*100;
}
current_loc.lng = packet.lon * T7;
current_loc.lat = packet.lat * T7;
current_loc.alt = g_gps->altitude - gps_base_alt;
if (!home_is_set) {
init_home();
}
break;
}
#if HIL_MODE == HIL_MODE_ATTITUDE
case MAVLINK_MSG_ID_ATTITUDE: //30
{
// decode
mavlink_attitude_t packet;
mavlink_msg_attitude_decode(msg, &packet);
// set AHRS hil sensor
ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
packet.pitchspeed,packet.yawspeed);
// rad/sec
/*
Vector3f gyros;
gyros.x = (float)packet.rollspeed;
gyros.y = (float)packet.pitchspeed;
gyros.z = (float)packet.yawspeed;
imu.set_gyro(gyros);
*/
//imu.set_accel(accels);
// rad/sec // JLN update - FOR HIL SIMULATION WITH AEROSIM
Vector3f gyros;
gyros.x = (float)packet.rollspeed / 1000.0;
gyros.y = (float)packet.pitchspeed / 1000.0;
gyros.z = (float)packet.yawspeed / 1000.0;
imu.set_gyro(gyros);
// m/s/s
Vector3f accels;
accels.x = (float)packet.roll * gravity / 1000.0;
accels.y = (float)packet.pitch * gravity / 1000.0;
accels.z = (float)packet.yaw * gravity / 1000.0;
imu.set_accel(accels);
break;
}
#endif // HIL_MODE_ATTITUDE
#endif // HIL_MODE != HIL_MODE_DISABLED && MAVLINK10 != ENABLED
#if MAVLINK10 == ENABLED && HIL_MODE != HIL_MODE_DISABLED
#if HIL_MODE != HIL_MODE_DISABLED
case MAVLINK_MSG_ID_HIL_STATE:
{
mavlink_hil_state_t packet;
@ -1968,7 +1667,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#endif // MAVLINK10 == ENABLED && HIL_MODE != HIL_MODE_DISABLED
#endif // HIL_MODE != HIL_MODE_DISABLED
/*
case MAVLINK_MSG_ID_HEARTBEAT:

18
ArduCopter/Makefile
View File

@ -21,33 +21,15 @@ heli:
apm2:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2"
apm2-mavlink10:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DMAVLINK10=ENABLED"
apm2-mavlink09:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DMAVLINK10=DISABLED"
apm2hexa:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DFRAME_CONFIG=HEXA_FRAME"
apm2beta:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DAPM2_BETA_HARDWARE"
mavlink10:
make -f Makefile EXTRAFLAGS="-DMAVLINK10=ENABLED"
mavlink09:
make -f Makefile EXTRAFLAGS="-DMAVLINK10=DISABLED"
sitl:
make -f ../libraries/Desktop/Makefile.desktop
sitl-mavlink10:
make -f ../libraries/Desktop/Makefile.desktop EXTRAFLAGS="-DMAVLINK10=ENABLED"
sitl-mavlink09:
make -f ../libraries/Desktop/Makefile.desktop EXTRAFLAGS="-DMAVLINK10=DISABLED"
sitl-octa:
make -f ../libraries/Desktop/Makefile.desktop octa

8
ArduCopter/config.h
View File

@ -67,14 +67,6 @@
# endif
#endif
//////////////////////////////////////////////////////////////////////////////
// MAVLINK10
//
#ifndef MAVLINK10
# define MAVLINK10 ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// FRAME_CONFIG
//

4
ArduCopter/options.cmake
View File

@ -97,10 +97,6 @@ apm_option("MAV_SYSTEM_ID" TYPE STRING ADVANCED
DESCRIPTION "MAVLink System ID?"
DEFAULT "1")
apm_option("MAVLINK10" TYPE BOOL DEFINE_ONLY BUILD_FLAG
DESCRIPTION "Use mavlink version 1.0?"
DEFAULT OFF)
apm_option("SERIAL0_BAUD" TYPE STRING ADVANCED
DESCRIPTION "Serial 0 baudrate?"
DEFAULT "115200"

287
ArduPlane/GCS_Mavlink.pde
View File

@ -27,7 +27,6 @@ static bool mavlink_active;
static NOINLINE void send_heartbeat(mavlink_channel_t chan)
{
#if MAVLINK10 == ENABLED
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t system_status = MAV_STATE_ACTIVE;
uint32_t custom_mode = control_mode;
@ -98,12 +97,6 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
base_mode,
custom_mode,
system_status);
#else // MAVLINK10
mavlink_msg_heartbeat_send(
chan,
mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA);
#endif // MAVLINK10
}
static NOINLINE void send_attitude(mavlink_channel_t chan)
@ -130,7 +123,6 @@ static NOINLINE void send_fence_status(mavlink_channel_t chan)
static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops)
{
#if MAVLINK10 == ENABLED
uint32_t control_sensors_present = 0;
uint32_t control_sensors_enabled;
uint32_t control_sensors_health;
@ -233,69 +225,6 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
0, // comm drops in pkts,
0, 0, 0, 0);
#else // MAVLINK10
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
switch(control_mode) {
case MANUAL:
mode = MAV_MODE_MANUAL;
break;
case STABILIZE:
mode = MAV_MODE_TEST1;
break;
case FLY_BY_WIRE_A:
mode = MAV_MODE_TEST2;
nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc.
break;
case FLY_BY_WIRE_B:
mode = MAV_MODE_TEST2;
nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc.
break;
case GUIDED:
mode = MAV_MODE_GUIDED;
break;
case AUTO:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
break;
case RTL:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
case LOITER:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LOITER;
break;
case INITIALISING:
mode = MAV_MODE_UNINIT;
nav_mode = MAV_NAV_GROUNDED;
break;
case CIRCLE:
mode = MAV_MODE_TEST3;
break;
}
uint8_t status = MAV_STATE_ACTIVE;
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total1)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
if (g.battery_monitoring == 3) {
/*setting a out-of-range value.
It informs to external devices that
it cannot be calculated properly just by voltage*/
battery_remaining = 1500;
}
mavlink_msg_sys_status_send(
chan,
mode,
nav_mode,
status,
load * 1000,
battery_voltage1 * 1000,
battery_remaining,
packet_drops);
#endif // MAVLINK10
}
static void NOINLINE send_meminfo(mavlink_channel_t chan)
@ -337,7 +266,6 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
{
#if MAVLINK10 == ENABLED
uint8_t fix = g_gps->status();
if (fix == GPS::GPS_OK) {
fix = 3;
@ -355,20 +283,6 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan)
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,
g_gps->last_fix_time*(uint64_t)1000,
g_gps->status(),
g_gps->latitude / 1.0e7,
g_gps->longitude / 1.0e7,
g_gps->altitude / 100.0,
g_gps->hdop,
0.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)
@ -591,11 +505,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
break;
case MSG_GPS_RAW:
#if MAVLINK10 == ENABLED
CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
#else
CHECK_PAYLOAD_SIZE(GPS_RAW);
#endif
send_gps_raw(chan);
break;
@ -1086,7 +996,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#if MAVLINK10 == ENABLED
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// decode
@ -1142,133 +1051,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#else // MAVLINK10
case MAVLINK_MSG_ID_ACTION:
{
// decode
mavlink_action_t packet;
mavlink_msg_action_decode(msg, &packet);
if (mavlink_check_target(packet.target,packet.target_component)) break;
uint8_t result = 0;
// do action
send_text(SEVERITY_LOW,PSTR("action received: "));
//Serial.println(packet.action);
switch(packet.action){
case MAV_ACTION_LAUNCH:
//set_mode(TAKEOFF);
break;
case MAV_ACTION_RETURN:
set_mode(RTL);
result=1;
break;
case MAV_ACTION_EMCY_LAND:
//set_mode(LAND);
break;
case MAV_ACTION_HALT:
do_loiter_at_location();
result=1;
break;
/* No mappable implementation in APM 2.0
case MAV_ACTION_MOTORS_START:
case MAV_ACTION_CONFIRM_KILL:
case MAV_ACTION_EMCY_KILL:
case MAV_ACTION_MOTORS_STOP:
case MAV_ACTION_SHUTDOWN:
break;
*/
case MAV_ACTION_CONTINUE:
process_next_command();
result=1;
break;
case MAV_ACTION_SET_MANUAL:
set_mode(MANUAL);
result=1;
break;
case MAV_ACTION_SET_AUTO:
set_mode(AUTO);
result=1;
// clearing failsafe should not be needed
// here. Added based on some puzzling results in
// the simulator (tridge)
failsafe = FAILSAFE_NONE;
break;
case MAV_ACTION_STORAGE_READ:
// we load all variables at startup, and
// save on each mavlink set
result=1;
break;
case MAV_ACTION_STORAGE_WRITE:
// this doesn't make any sense, as we save
// all settings as they come in
result=1;
break;
case MAV_ACTION_CALIBRATE_RC: break;
trim_radio();
result=1;
break;
case MAV_ACTION_CALIBRATE_GYRO:
case MAV_ACTION_CALIBRATE_MAG:
case MAV_ACTION_CALIBRATE_ACC:
case MAV_ACTION_CALIBRATE_PRESSURE:
case MAV_ACTION_REBOOT: // this is a rough interpretation
startup_IMU_ground(true);
result=1;
break;
/* For future implemtation
case MAV_ACTION_REC_START: break;
case MAV_ACTION_REC_PAUSE: break;
case MAV_ACTION_REC_STOP: break;
*/
/* Takeoff is not an implemented flight mode in APM 2.0
case MAV_ACTION_TAKEOFF:
set_mode(TAKEOFF);
break;
*/
case MAV_ACTION_NAVIGATE:
set_mode(AUTO);
result=1;
break;
/* Land is not an implemented flight mode in APM 2.0
case MAV_ACTION_LAND:
set_mode(LAND);
break;
*/
case MAV_ACTION_LOITER:
set_mode(LOITER);
result=1;
break;
default: break;
}
mavlink_msg_action_ack_send(
chan,
packet.action,
result
);
break;
}
#endif
case MAVLINK_MSG_ID_SET_MODE:
{
@ -1276,7 +1058,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_set_mode_t packet;
mavlink_msg_set_mode_decode(msg, &packet);
#if MAVLINK10 == ENABLED
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
@ -1297,54 +1078,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#else // MAVLINK10
switch(packet.mode){
case MAV_MODE_MANUAL:
set_mode(MANUAL);
break;
case MAV_MODE_GUIDED:
set_mode(GUIDED);
break;
case MAV_MODE_AUTO:
if(mav_nav == 255 || mav_nav == MAV_NAV_WAYPOINT) set_mode(AUTO);
if(mav_nav == MAV_NAV_RETURNING) set_mode(RTL);
if(mav_nav == MAV_NAV_LOITER) set_mode(LOITER);
mav_nav = 255;
break;
case MAV_MODE_TEST1:
set_mode(STABILIZE);
break;
case MAV_MODE_TEST2:
if(mav_nav == 255 || mav_nav == 1) set_mode(FLY_BY_WIRE_A);
if(mav_nav == 2) set_mode(FLY_BY_WIRE_B);
//if(mav_nav == 3) set_mode(FLY_BY_WIRE_C);
mav_nav = 255;
break;
}
#endif
break;
}
#if MAVLINK10 == DISABLED
case MAVLINK_MSG_ID_SET_NAV_MODE:
{
// decode
mavlink_set_nav_mode_t packet;
mavlink_msg_set_nav_mode_decode(msg, &packet);
// To set some flight modes we must first receive a "set nav mode" message and then a "set mode" message
mav_nav = packet.nav_mode;
break;
}
#endif // MAVLINK10
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
{
// decode
@ -1684,9 +1420,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
default:
#if MAVLINK10 == ENABLED
result = MAV_MISSION_UNSUPPORTED;
#endif
break;
}
@ -1897,10 +1631,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#if HIL_MODE != HIL_MODE_DISABLED
#if HIL_MODE != HIL_MODE_DISABLED
// This is used both as a sensor and to pass the location
// in HIL_ATTITUDE mode.
#if MAVLINK10 == ENABLED
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
// decode
@ -1913,19 +1646,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
packet.vel*1.0e-2, packet.cog*1.0e-2, 0, 10);
break;
}
#else // MAVLINK10
case MAVLINK_MSG_ID_GPS_RAW:
{
// decode
mavlink_gps_raw_t packet;
mavlink_msg_gps_raw_decode(msg, &packet);
// set gps hil sensor
g_gps->setHIL(packet.usec/1000,packet.lat,packet.lon,packet.alt,
packet.v,packet.hdg,0,10);
break;
}
#endif // MAVLINK10
// Is this resolved? - MAVLink protocol change.....
case MAVLINK_MSG_ID_VFR_HUD:
@ -1938,7 +1658,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
airspeed = 100*packet.airspeed;
break;
}
#if MAVLINK10 == ENABLED
case MAVLINK_MSG_ID_HIL_STATE:
{
mavlink_hil_state_t packet;
@ -1979,8 +1699,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#endif // MAVLINK10
#endif
#endif // HIL_MODE
#if HIL_MODE == HIL_MODE_ATTITUDE
case MAVLINK_MSG_ID_ATTITUDE:
{

18
ArduPlane/Makefile
View File

@ -34,27 +34,9 @@ apm2beta:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DAPM2_BETA_HARDWARE"
apm2-mavlink10:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DMAVLINK10=ENABLED"
apm2-mavlink09:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DMAVLINK10=DISABLED"
mavlink10:
make -f Makefile EXTRAFLAGS="-DMAVLINK10=ENABLED"
mavlink09:
make -f Makefile EXTRAFLAGS="-DMAVLINK10=DISABLED"
sitl:
make -f ../libraries/Desktop/Makefile.desktop
sitl-mavlink10:
make -f ../libraries/Desktop/Makefile.desktop EXTRAFLAGS="-DMAVLINK10=ENABLED"
sitl-mavlink09:
make -f ../libraries/Desktop/Makefile.desktop EXTRAFLAGS="-DMAVLINK10=DISABLED"
sitl-mount:
make -f ../libraries/Desktop/Makefile.desktop EXTRAFLAGS="-DMOUNT=ENABLED"

7
ArduPlane/config.h
View File

@ -121,13 +121,6 @@
#endif
//////////////////////////////////////////////////////////////////////////////
// MAVLINK10
//
#ifndef MAVLINK10
# define MAVLINK10 ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// AIRSPEED_SENSOR
// AIRSPEED_RATIO

4
ArduPlane/options.cmake
View File

@ -97,10 +97,6 @@ apm_option("MAV_SYSTEM_ID" TYPE STRING ADVANCED
DESCRIPTION "MAVLink System ID?"
DEFAULT "1")
apm_option("MAVLINK10" TYPE BOOL DEFINE_ONLY BUILD_FLAG
DESCRIPTION "Use mavlink version 1.0?"
DEFAULT OFF)
apm_option("SERIAL0_BAUD" TYPE STRING ADVANCED
DESCRIPTION "Serial 0 baudrate?"
DEFAULT "115200"

2
Tools/autotest/pysim/util.py
View File

@ -59,8 +59,6 @@ def deltree(path):
def build_SIL(atype, target='sitl'):
'''build desktop SIL'''
if os.getenv('MAVLINK10'):
target += '-mavlink10'
run_cmd("make clean %s" % target,
dir=reltopdir(atype),
checkfail=True)

2
Tools/scripts/build_all.sh
View File

@ -8,7 +8,7 @@ set -x
echo "Testing ArduPlane build"
pushd ArduPlane
for b in all apm2 apm2beta hil hilsensors mavlink10 sitl sitl-mavlink10 sitl-mount; do
for b in all apm2 apm2beta hil hilsensors sitl sitl-mount; do
pwd
make clean
make $b

3
libraries/Desktop/Makefile.desktop
View File

@ -25,6 +25,3 @@ hexa:
y6:
make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=Y6_FRAME"
mavlink10:
make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DMAVLINK10"

23
libraries/GCS_MAVLink/GCS_MAVLink.h
View File

@ -12,26 +12,13 @@
// to select MAVLink 1.0 in the arduino GUI build
//#define MAVLINK_SEPARATE_HELPERS
#ifndef MAVLINK10
// default to MAVLINK 1.0
#define MAVLINK10 ENABLED
#endif
#if MAVLINK10 == ENABLED
# include "include/mavlink/v1.0/ardupilotmega/version.h"
#else
# include "include/mavlink/v0.9/ardupilotmega/version.h"
#endif
#include "include/mavlink/v1.0/ardupilotmega/version.h"
// this allows us to make mavlink_message_t much smaller
#define MAVLINK_MAX_PAYLOAD_LEN MAVLINK_MAX_DIALECT_PAYLOAD_SIZE
#define MAVLINK_COMM_NUM_BUFFERS 2
#if MAVLINK10==ENABLED
# include "include/mavlink/v1.0/mavlink_types.h"
#else
# include "include/mavlink/v0.9/mavlink_types.h"
#endif
#include "include/mavlink/v1.0/mavlink_types.h"
/// MAVLink stream used for HIL interaction
extern BetterStream *mavlink_comm_0_port;
@ -124,11 +111,7 @@ static inline int comm_get_txspace(mavlink_channel_t chan)
}
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
#if MAVLINK10==ENABLED
# include "include/mavlink/v1.0/ardupilotmega/mavlink.h"
#else
# include "include/mavlink/v0.9/ardupilotmega/mavlink.h"
#endif
#include "include/mavlink/v1.0/ardupilotmega/mavlink.h"
uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid);

101
libraries/GCS_MAVLink/Mavlink_compat.h
View File

@ -2,7 +2,6 @@
compatibility header during transition to MAVLink 1.0
*/
#if MAVLINK10==1
// in MAVLink 1.0 'waypoint' becomes 'mission'. We can remove these
// mappings once we are not trying to support both protocols
@ -72,103 +71,3 @@ static uint8_t mav_var_type(enum ap_var_type t)
#define MAV_FIXED_WING MAV_TYPE_FIXED_WING
#else // MAVLINK10
static uint8_t mav_var_type(enum ap_var_type t)
{
return 0;
}
#define MAV_MISSION_ACCEPTED 0
#define MAV_MISSION_UNSUPPORTED 1
#define MAV_MISSION_UNSUPPORTED_FRAME 1
#define MAV_MISSION_ERROR 1
#define MAV_MISSION_INVALID_SEQUENCE 1
/*
some functions have some extra params in MAVLink 1.0
*/
static void mavlink_msg_global_position_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat, int32_t lon,
int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy,
int16_t vz, uint16_t hdg)
{
mavlink_msg_global_position_int_send(
chan,
lat,
lon,
alt,
vx, vy, vz);
}
static void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port,
int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled,
int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled,
int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi)
{
mavlink_msg_rc_channels_scaled_send(
chan,
chan1_scaled,
chan2_scaled,
chan3_scaled,
chan4_scaled,
chan5_scaled,
chan6_scaled,
chan7_scaled,
chan8_scaled,
rssi);
}
static void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port,
uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw,
uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw,
uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi)
{
mavlink_msg_rc_channels_raw_send(
chan,
chan1_raw,
chan2_raw,
chan3_raw,
chan4_raw,
chan5_raw,
chan6_raw,
chan7_raw,
chan8_raw,
rssi);
}
static void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint32_t time_usec, uint8_t port,
uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw,
uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw,
uint16_t servo7_raw, uint16_t servo8_raw)
{
mavlink_msg_servo_output_raw_send(
chan,
servo1_raw,
servo2_raw,
servo3_raw,
servo4_raw,
servo5_raw,
servo6_raw,
servo7_raw,
servo8_raw);
}
static void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char *text)
{
mavlink_msg_statustext_send(chan, severity, (const int8_t*) text);
}
static void mavlink_msg_param_value_send(mavlink_channel_t chan, const char *param_id,
float param_value, uint8_t param_type,
uint16_t param_count, uint16_t param_index)
{
mavlink_msg_param_value_send(
chan,
(int8_t *)param_id,
param_value,
param_count,
param_index);
}
#endif // MAVLINK10