aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

use C++ tricks to minimise differences for MAVLink 1.0

Browse Source 
this should make maintainence/testing a bit easier
This commit is contained in:
Andrew Tridgell 2011-10-29 19:13:55 +11:00
parent 8102b31d8d
commit 580647b1de
2 changed files with 91 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
ArduPlane/GCS_Mavlink.pde
View File

@ -284,7 +284,6 @@ static void NOINLINE send_meminfo(mavlink_channel_t chan)
static void NOINLINE send_location(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 Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
#ifdef MAVLINK10
mavlink_msg_global_position_int_send( mavlink_msg_global_position_int_send(
chan, chan,
millis(), 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.b.x, // Y speed cm/s
g_gps->ground_speed * rot.c.x, g_gps->ground_speed * rot.c.x,
g_gps->ground_course); // course in 1/100 degree 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) 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 // normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with // This is used for HIL. Do not change without discussing with
// HIL maintainers // HIL maintainers
#ifdef MAVLINK10
mavlink_msg_rc_channels_scaled_send( mavlink_msg_rc_channels_scaled_send(
chan, chan,
millis(), millis(),
@ -380,26 +368,11 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
0, 0,
0, 0,
rssi); 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) static void NOINLINE send_radio_in(mavlink_channel_t chan)
{ {
uint8_t rssi = 1; uint8_t rssi = 1;
#ifdef MAVLINK10
mavlink_msg_rc_channels_raw_send( mavlink_msg_rc_channels_raw_send(
chan, chan,
millis(), millis(),
@ -413,25 +386,10 @@ static void NOINLINE send_radio_in(mavlink_channel_t chan)
g.rc_7.radio_in, g.rc_7.radio_in,
g.rc_8.radio_in, g.rc_8.radio_in,
rssi); 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) static void NOINLINE send_radio_out(mavlink_channel_t chan)
{ {
#ifdef MAVLINK10
mavlink_msg_servo_output_raw_send( mavlink_msg_servo_output_raw_send(
chan, chan,
micros(), micros(),
@ -444,18 +402,6 @@ static void NOINLINE send_radio_out(mavlink_channel_t chan)
g.rc_6.radio_out, g.rc_6.radio_out,
g.rc_7.radio_out, g.rc_7.radio_out,
g.rc_8.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) 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); mavlink_send_message(chan, MSG_STATUSTEXT, 0);
} else { } else {
// send immediately // send immediately
#ifdef MAVLINK10
mavlink_msg_statustext_send(chan, severity, str); 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 // we send the value we actually set, which could be
// different from the value sent, in case someone sent // different from the value sent, in case someone sent
// a fractional value to an integer type // a fractional value to an integer type
#ifdef MAVLINK10
mavlink_msg_param_value_send( mavlink_msg_param_value_send(
chan, chan,
key, key,
@ -1739,14 +1680,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mav_var_type(vp->meta_type_id()), mav_var_type(vp->meta_type_id()),
_count_parameters(), _count_parameters(),
-1); // XXX we don't actually know what its index is... -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; break;
@ -1959,7 +1892,6 @@ GCS_MAVLINK::queued_param_send()
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
vp->copy_name(param_name, sizeof(param_name)); vp->copy_name(param_name, sizeof(param_name));
#ifdef MAVLINK10
mavlink_msg_param_value_send( mavlink_msg_param_value_send(
chan, chan,
param_name, param_name,
@ -1967,14 +1899,6 @@ GCS_MAVLINK::queued_param_send()
mav_var_type(vp->meta_type_id()), mav_var_type(vp->meta_type_id()),
_queued_parameter_count, _queued_parameter_count,
_queued_parameter_index); _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++; _queued_parameter_index++;
} }

91
ArduPlane/Mavlink_compat.h
View File

@ -72,10 +72,101 @@ static uint8_t mav_var_type(AP_Meta_class::Type_id t)
#else #else
static uint8_t mav_var_type(AP_Meta_class::Type_id t)
{
return 0;
}
#define MAV_MISSION_ACCEPTED 0 #define MAV_MISSION_ACCEPTED 0
#define MAV_MISSION_UNSUPPORTED 1 #define MAV_MISSION_UNSUPPORTED 1
#define MAV_MISSION_UNSUPPORTED_FRAME 1 #define MAV_MISSION_UNSUPPORTED_FRAME 1
#define MAV_MISSION_ERROR 1 #define MAV_MISSION_ERROR 1
#define MAV_MISSION_INVALID_SEQUENCE 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 #endif