From c914b1150491c5e55fb4ba38d0aa593b0fef87c9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 29 Oct 2011 19:13:55 +1100 Subject: [PATCH] use C++ tricks to minimise differences for MAVLink 1.0 this should make maintainence/testing a bit easier --- ArduPlane/GCS_Mavlink.pde | 76 ------------------------------- ArduPlane/Mavlink_compat.h | 91 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 91 insertions(+), 76 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 22bbd0dfe3..ee6d7d9169 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -284,7 +284,6 @@ static void NOINLINE send_meminfo(mavlink_channel_t chan) static void NOINLINE send_location(mavlink_channel_t chan) { Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now -#ifdef MAVLINK10 mavlink_msg_global_position_int_send( chan, millis(), @@ -296,16 +295,6 @@ static void NOINLINE send_location(mavlink_channel_t chan) 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 -#else // MAVLINK10 - 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); -#endif // MAVLINK10 } static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) @@ -366,7 +355,6 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan) // normalized values scaled to -10000 to 10000 // This is used for HIL. Do not change without discussing with // HIL maintainers -#ifdef MAVLINK10 mavlink_msg_rc_channels_scaled_send( chan, millis(), @@ -380,26 +368,11 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan) 0, 0, rssi); - -#else // MAVLINK10 - mavlink_msg_rc_channels_scaled_send( - chan, - 10000 * g.channel_roll.norm_output(), - 10000 * g.channel_pitch.norm_output(), - 10000 * g.channel_throttle.norm_output(), - 10000 * g.channel_rudder.norm_output(), - 0, - 0, - 0, - 0, - rssi); -#endif // MAVLINK10 } static void NOINLINE send_radio_in(mavlink_channel_t chan) { uint8_t rssi = 1; -#ifdef MAVLINK10 mavlink_msg_rc_channels_raw_send( chan, millis(), @@ -413,25 +386,10 @@ static void NOINLINE send_radio_in(mavlink_channel_t chan) g.rc_7.radio_in, g.rc_8.radio_in, rssi); - -#else // MAVLINK10 - mavlink_msg_rc_channels_raw_send( - chan, - g.channel_roll.radio_in, - g.channel_pitch.radio_in, - g.channel_throttle.radio_in, - g.channel_rudder.radio_in, - g.rc_5.radio_in, // XXX currently only 4 RC channels defined - g.rc_6.radio_in, - g.rc_7.radio_in, - g.rc_8.radio_in, - rssi); -#endif // MAVLINK10 } static void NOINLINE send_radio_out(mavlink_channel_t chan) { -#ifdef MAVLINK10 mavlink_msg_servo_output_raw_send( chan, micros(), @@ -444,18 +402,6 @@ static void NOINLINE send_radio_out(mavlink_channel_t chan) g.rc_6.radio_out, g.rc_7.radio_out, g.rc_8.radio_out); -#else // MAVLINK10 - mavlink_msg_servo_output_raw_send( - chan, - g.channel_roll.radio_out, - g.channel_pitch.radio_out, - g.channel_throttle.radio_out, - g.channel_rudder.radio_out, - g.rc_5.radio_out, // XXX currently only 4 RC channels defined - g.rc_6.radio_out, - g.rc_7.radio_out, - g.rc_8.radio_out); -#endif // MAVLINK10 } static void NOINLINE send_vfr_hud(mavlink_channel_t chan) @@ -748,11 +694,7 @@ void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char mavlink_send_message(chan, MSG_STATUSTEXT, 0); } else { // send immediately -#ifdef MAVLINK10 mavlink_msg_statustext_send(chan, severity, str); -#else - mavlink_msg_statustext_send(chan, severity, (const int8_t*) str); -#endif } } @@ -1731,7 +1673,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // we send the value we actually set, which could be // different from the value sent, in case someone sent // a fractional value to an integer type -#ifdef MAVLINK10 mavlink_msg_param_value_send( chan, key, @@ -1739,14 +1680,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mav_var_type(vp->meta_type_id()), _count_parameters(), -1); // XXX we don't actually know what its index is... -#else // MAVLINK10 - mavlink_msg_param_value_send( - chan, - (int8_t *)key, - vp->cast_to_float(), - _count_parameters(), - -1); // XXX we don't actually know what its index is... -#endif // MAVLINK10 } break; @@ -1959,7 +1892,6 @@ GCS_MAVLINK::queued_param_send() char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK vp->copy_name(param_name, sizeof(param_name)); -#ifdef MAVLINK10 mavlink_msg_param_value_send( chan, param_name, @@ -1967,14 +1899,6 @@ GCS_MAVLINK::queued_param_send() mav_var_type(vp->meta_type_id()), _queued_parameter_count, _queued_parameter_index); -#else // MAVLINK10 - mavlink_msg_param_value_send( - chan, - (int8_t*)param_name, - value, - _queued_parameter_count, - _queued_parameter_index); -#endif // MAVLINK10 _queued_parameter_index++; } diff --git a/ArduPlane/Mavlink_compat.h b/ArduPlane/Mavlink_compat.h index a9884bf780..267c73832e 100644 --- a/ArduPlane/Mavlink_compat.h +++ b/ArduPlane/Mavlink_compat.h @@ -72,10 +72,101 @@ static uint8_t mav_var_type(AP_Meta_class::Type_id t) #else +static uint8_t mav_var_type(AP_Meta_class::Type_id 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