mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
use C++ tricks to minimise differences for MAVLink 1.0
this should make maintainence/testing a bit easier
This commit is contained in:
Andrew Tridgell
parent
b655482b98
commit
c914b11504
@ -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++;
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user