mirror of https://github.com/ArduPilot/ardupilot
remove Mavlink_Common.h and used deferred logic for params/waypoints
this moves the mavlink send logic into GCS_Mavlink.pde, and also ensures we only ever send parameters and waypoints when there is sufficient space in the serial send buffer
This commit is contained in:
parent
ef6e2c2adf
commit
225e6d760f
|
@ -131,12 +131,14 @@ public:
|
||||||
void send_text(uint8_t severity, const char *str);
|
void send_text(uint8_t severity, const char *str);
|
||||||
void send_text(uint8_t severity, const prog_char_t *str);
|
void send_text(uint8_t severity, const prog_char_t *str);
|
||||||
void data_stream_send(uint16_t freqMin, uint16_t freqMax);
|
void data_stream_send(uint16_t freqMin, uint16_t freqMax);
|
||||||
|
void queued_param_send();
|
||||||
|
void queued_waypoint_send();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void handleMessage(mavlink_message_t * msg);
|
void handleMessage(mavlink_message_t * msg);
|
||||||
|
|
||||||
/// Perform queued sending operations
|
/// Perform queued sending operations
|
||||||
///
|
///
|
||||||
void _queued_send();
|
|
||||||
|
|
||||||
AP_Var *_queued_parameter; ///< next parameter to be sent in queue
|
AP_Var *_queued_parameter; ///< next parameter to be sent in queue
|
||||||
uint16_t _queued_parameter_index; ///< next queued parameter's index
|
uint16_t _queued_parameter_index; ///< next queued parameter's index
|
||||||
|
@ -160,7 +162,6 @@ private:
|
||||||
uint16_t packet_drops;
|
uint16_t packet_drops;
|
||||||
|
|
||||||
// waypoints
|
// waypoints
|
||||||
uint16_t requested_interface; // request port to use
|
|
||||||
uint16_t waypoint_request_i; // request index
|
uint16_t waypoint_request_i; // request index
|
||||||
uint16_t waypoint_dest_sysid; // where to send requests
|
uint16_t waypoint_dest_sysid; // where to send requests
|
||||||
uint16_t waypoint_dest_compid; // "
|
uint16_t waypoint_dest_compid; // "
|
||||||
|
|
|
@ -1,12 +1,475 @@
|
||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
|
||||||
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
|
|
||||||
|
|
||||||
#include "Mavlink_Common.h"
|
|
||||||
|
|
||||||
// use this to prevent recursion during sensor init
|
// use this to prevent recursion during sensor init
|
||||||
static bool in_mavlink_delay;
|
static bool in_mavlink_delay;
|
||||||
|
|
||||||
|
static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
|
||||||
|
{
|
||||||
|
if (sysid != mavlink_system.sysid)
|
||||||
|
return 1;
|
||||||
|
// Currently we are not checking for correct compid since APM is not passing mavlink info to any subsystem
|
||||||
|
// If it is addressed to our system ID we assume it is for us
|
||||||
|
return 0; // no error
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// check if a message will fit in the payload space available
|
||||||
|
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
|
||||||
|
|
||||||
|
/*
|
||||||
|
!!NOTE!!
|
||||||
|
|
||||||
|
the use of NOINLINE separate functions for each message type avoids
|
||||||
|
a compiler bug in gcc that would cause it to use far more stack
|
||||||
|
space than is needed. Without the NOINLINE we use the sum of the
|
||||||
|
stack needed for each message type. Please be careful to follow the
|
||||||
|
pattern below when adding any new messages
|
||||||
|
*/
|
||||||
|
|
||||||
|
static NOINLINE void send_heartbeat(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
mavlink_msg_heartbeat_send(
|
||||||
|
chan,
|
||||||
|
mavlink_system.type,
|
||||||
|
MAV_AUTOPILOT_ARDUPILOTMEGA);
|
||||||
|
}
|
||||||
|
|
||||||
|
static NOINLINE void send_attitude(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
Vector3f omega = dcm.get_gyro();
|
||||||
|
mavlink_msg_attitude_send(
|
||||||
|
chan,
|
||||||
|
micros(),
|
||||||
|
dcm.roll,
|
||||||
|
dcm.pitch,
|
||||||
|
dcm.yaw,
|
||||||
|
omega.x,
|
||||||
|
omega.y,
|
||||||
|
omega.z);
|
||||||
|
}
|
||||||
|
|
||||||
|
static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops)
|
||||||
|
{
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t status = MAV_STATE_ACTIVE;
|
||||||
|
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
|
||||||
|
|
||||||
|
mavlink_msg_sys_status_send(
|
||||||
|
chan,
|
||||||
|
mode,
|
||||||
|
nav_mode,
|
||||||
|
status,
|
||||||
|
load * 1000,
|
||||||
|
battery_voltage * 1000,
|
||||||
|
battery_remaining,
|
||||||
|
packet_drops);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void NOINLINE send_meminfo(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
extern unsigned __brkval;
|
||||||
|
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
|
||||||
|
}
|
||||||
|
|
||||||
|
static void NOINLINE send_location(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
Matrix3f rot = dcm.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);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
mavlink_msg_nav_controller_output_send(
|
||||||
|
chan,
|
||||||
|
nav_roll / 1.0e2,
|
||||||
|
nav_pitch / 1.0e2,
|
||||||
|
nav_bearing / 1.0e2,
|
||||||
|
target_bearing / 1.0e2,
|
||||||
|
wp_distance,
|
||||||
|
altitude_error / 1.0e2,
|
||||||
|
airspeed_error,
|
||||||
|
crosstrack_error);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void NOINLINE send_servo_out(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
const uint8_t rssi = 1;
|
||||||
|
// normalized values scaled to -10000 to 10000
|
||||||
|
// This is used for HIL. Do not change without discussing with HIL maintainers
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void NOINLINE send_radio_in(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
uint8_t rssi = 1;
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void NOINLINE send_radio_out(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
mavlink_msg_vfr_hud_send(
|
||||||
|
chan,
|
||||||
|
(float)airspeed / 100.0,
|
||||||
|
(float)g_gps->ground_speed / 100.0,
|
||||||
|
(dcm.yaw_sensor / 100) % 360,
|
||||||
|
(int)g.channel_throttle.servo_out,
|
||||||
|
current_loc.alt / 100.0,
|
||||||
|
climb_rate);
|
||||||
|
}
|
||||||
|
|
||||||
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
|
static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
Vector3f accel = imu.get_accel();
|
||||||
|
Vector3f gyro = imu.get_gyro();
|
||||||
|
|
||||||
|
mavlink_msg_raw_imu_send(
|
||||||
|
chan,
|
||||||
|
micros(),
|
||||||
|
accel.x * 1000.0 / gravity,
|
||||||
|
accel.y * 1000.0 / gravity,
|
||||||
|
accel.z * 1000.0 / gravity,
|
||||||
|
gyro.x * 1000.0,
|
||||||
|
gyro.y * 1000.0,
|
||||||
|
gyro.z * 1000.0,
|
||||||
|
compass.mag_x,
|
||||||
|
compass.mag_y,
|
||||||
|
compass.mag_z);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
mavlink_msg_scaled_pressure_send(
|
||||||
|
chan,
|
||||||
|
micros(),
|
||||||
|
(float)barometer.Press/100.0,
|
||||||
|
(float)(barometer.Press-g.ground_pressure)/100.0,
|
||||||
|
(int)(barometer.Temp*10));
|
||||||
|
}
|
||||||
|
|
||||||
|
static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
Vector3f mag_offsets = compass.get_offsets();
|
||||||
|
|
||||||
|
mavlink_msg_sensor_offsets_send(chan,
|
||||||
|
mag_offsets.x,
|
||||||
|
mag_offsets.y,
|
||||||
|
mag_offsets.z,
|
||||||
|
compass.get_declination(),
|
||||||
|
barometer.RawPress,
|
||||||
|
barometer.RawTemp,
|
||||||
|
imu.gx(), imu.gy(), imu.gz(),
|
||||||
|
imu.ax(), imu.ay(), imu.az());
|
||||||
|
}
|
||||||
|
#endif // HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
|
|
||||||
|
static void NOINLINE send_gps_status(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
mavlink_msg_gps_status_send(
|
||||||
|
chan,
|
||||||
|
g_gps->num_sats,
|
||||||
|
NULL,
|
||||||
|
NULL,
|
||||||
|
NULL,
|
||||||
|
NULL,
|
||||||
|
NULL);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
mavlink_msg_waypoint_current_send(
|
||||||
|
chan,
|
||||||
|
g.waypoint_index);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// try to send a message, return false if it won't fit in the serial tx buffer
|
||||||
|
static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
|
||||||
|
{
|
||||||
|
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||||
|
|
||||||
|
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
|
||||||
|
// defer any messages on the telemetry port for 1 second after
|
||||||
|
// bootup, to try to prevent bricking of Xbees
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
switch (id) {
|
||||||
|
case MSG_HEARTBEAT:
|
||||||
|
CHECK_PAYLOAD_SIZE(HEARTBEAT);
|
||||||
|
send_heartbeat(chan);
|
||||||
|
return true;
|
||||||
|
|
||||||
|
case MSG_EXTENDED_STATUS1:
|
||||||
|
CHECK_PAYLOAD_SIZE(SYS_STATUS);
|
||||||
|
send_extended_status1(chan, packet_drops);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_EXTENDED_STATUS2:
|
||||||
|
CHECK_PAYLOAD_SIZE(MEMINFO);
|
||||||
|
send_meminfo(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_ATTITUDE:
|
||||||
|
CHECK_PAYLOAD_SIZE(ATTITUDE);
|
||||||
|
send_attitude(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_LOCATION:
|
||||||
|
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
|
||||||
|
send_location(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_NAV_CONTROLLER_OUTPUT:
|
||||||
|
if (control_mode != MANUAL) {
|
||||||
|
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
|
||||||
|
send_nav_controller_output(chan);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_GPS_RAW:
|
||||||
|
CHECK_PAYLOAD_SIZE(GPS_RAW);
|
||||||
|
send_gps_raw(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_SERVO_OUT:
|
||||||
|
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
|
||||||
|
send_servo_out(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_RADIO_IN:
|
||||||
|
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
|
||||||
|
send_radio_in(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_RADIO_OUT:
|
||||||
|
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
|
||||||
|
send_radio_out(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_VFR_HUD:
|
||||||
|
CHECK_PAYLOAD_SIZE(VFR_HUD);
|
||||||
|
send_vfr_hud(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
|
case MSG_RAW_IMU1:
|
||||||
|
CHECK_PAYLOAD_SIZE(RAW_IMU);
|
||||||
|
send_raw_imu1(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_RAW_IMU2:
|
||||||
|
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
|
||||||
|
send_raw_imu2(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_RAW_IMU3:
|
||||||
|
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
|
||||||
|
send_raw_imu3(chan);
|
||||||
|
break;
|
||||||
|
#endif // HIL_MODE != HIL_MODE_ATTITUDE
|
||||||
|
|
||||||
|
case MSG_GPS_STATUS:
|
||||||
|
CHECK_PAYLOAD_SIZE(GPS_STATUS);
|
||||||
|
send_gps_status(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_CURRENT_WAYPOINT:
|
||||||
|
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
|
||||||
|
send_current_waypoint(chan);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_NEXT_PARAM:
|
||||||
|
CHECK_PAYLOAD_SIZE(PARAM_VALUE);
|
||||||
|
if (chan == MAVLINK_COMM_0) {
|
||||||
|
gcs0.queued_param_send();
|
||||||
|
} else {
|
||||||
|
gcs3.queued_param_send();
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_NEXT_WAYPOINT:
|
||||||
|
CHECK_PAYLOAD_SIZE(WAYPOINT_REQUEST);
|
||||||
|
if (chan == MAVLINK_COMM_0) {
|
||||||
|
gcs0.queued_waypoint_send();
|
||||||
|
} else {
|
||||||
|
gcs3.queued_waypoint_send();
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MSG_RETRY_DEFERRED:
|
||||||
|
break; // just here to prevent a warning
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED
|
||||||
|
static struct mavlink_queue {
|
||||||
|
enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES];
|
||||||
|
uint8_t next_deferred_message;
|
||||||
|
uint8_t num_deferred_messages;
|
||||||
|
} mavlink_queue[2];
|
||||||
|
|
||||||
|
// send a message using mavlink
|
||||||
|
static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
|
||||||
|
{
|
||||||
|
uint8_t i, nextid;
|
||||||
|
struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan];
|
||||||
|
|
||||||
|
// see if we can send the deferred messages, if any
|
||||||
|
while (q->num_deferred_messages != 0) {
|
||||||
|
if (!mavlink_try_send_message(chan,
|
||||||
|
q->deferred_messages[q->next_deferred_message],
|
||||||
|
packet_drops)) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
q->next_deferred_message++;
|
||||||
|
if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) {
|
||||||
|
q->next_deferred_message = 0;
|
||||||
|
}
|
||||||
|
q->num_deferred_messages--;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (id == MSG_RETRY_DEFERRED) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// this message id might already be deferred
|
||||||
|
for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) {
|
||||||
|
if (q->deferred_messages[nextid] == id) {
|
||||||
|
// its already deferred, discard
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
nextid++;
|
||||||
|
if (nextid == MAX_DEFERRED_MESSAGES) {
|
||||||
|
nextid = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (q->num_deferred_messages != 0 ||
|
||||||
|
!mavlink_try_send_message(chan, id, packet_drops)) {
|
||||||
|
// can't send it now, so defer it
|
||||||
|
if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) {
|
||||||
|
// the defer buffer is full, discard
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
nextid = q->next_deferred_message + q->num_deferred_messages;
|
||||||
|
if (nextid >= MAX_DEFERRED_MESSAGES) {
|
||||||
|
nextid -= MAX_DEFERRED_MESSAGES;
|
||||||
|
}
|
||||||
|
q->deferred_messages[nextid] = id;
|
||||||
|
q->num_deferred_messages++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str)
|
||||||
|
{
|
||||||
|
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
|
||||||
|
// don't send status MAVLink messages for 1 second after
|
||||||
|
// bootup, to try to prevent Xbee bricking
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
mavlink_msg_statustext_send(
|
||||||
|
chan,
|
||||||
|
severity,
|
||||||
|
(const int8_t*) str);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) :
|
GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) :
|
||||||
packet_drops(0),
|
packet_drops(0),
|
||||||
|
@ -35,7 +498,7 @@ void
|
||||||
GCS_MAVLINK::init(FastSerial * port)
|
GCS_MAVLINK::init(FastSerial * port)
|
||||||
{
|
{
|
||||||
GCS_Class::init(port);
|
GCS_Class::init(port);
|
||||||
if (port == &Serial) { // to split hil vs gcs
|
if (port == &Serial) {
|
||||||
mavlink_comm_0_port = port;
|
mavlink_comm_0_port = port;
|
||||||
chan = MAVLINK_COMM_0;
|
chan = MAVLINK_COMM_0;
|
||||||
}else{
|
}else{
|
||||||
|
@ -66,17 +529,22 @@ GCS_MAVLINK::update(void)
|
||||||
packet_drops += status.packet_rx_drop_count;
|
packet_drops += status.packet_rx_drop_count;
|
||||||
|
|
||||||
// send out queued params/ waypoints
|
// send out queued params/ waypoints
|
||||||
_queued_send();
|
if (NULL != _queued_parameter) {
|
||||||
|
send_message(MSG_NEXT_PARAM);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (waypoint_receiving &&
|
||||||
|
waypoint_request_i <= (unsigned)g.waypoint_total) {
|
||||||
|
send_message(MSG_NEXT_WAYPOINT);
|
||||||
|
}
|
||||||
|
|
||||||
// stop waypoint sending if timeout
|
// stop waypoint sending if timeout
|
||||||
if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){
|
if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){
|
||||||
send_text_P(SEVERITY_LOW,PSTR("waypoint send timeout"));
|
|
||||||
waypoint_sending = false;
|
waypoint_sending = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// stop waypoint receiving if timeout
|
// stop waypoint receiving if timeout
|
||||||
if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){
|
if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){
|
||||||
send_text_P(SEVERITY_LOW,PSTR("waypoint receive timeout"));
|
|
||||||
waypoint_receiving = false;
|
waypoint_receiving = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -90,7 +558,6 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
|
||||||
send_message(MSG_RAW_IMU1);
|
send_message(MSG_RAW_IMU1);
|
||||||
send_message(MSG_RAW_IMU2);
|
send_message(MSG_RAW_IMU2);
|
||||||
send_message(MSG_RAW_IMU3);
|
send_message(MSG_RAW_IMU3);
|
||||||
//Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get());
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
|
if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
|
||||||
|
@ -100,44 +567,39 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
|
||||||
send_message(MSG_CURRENT_WAYPOINT);
|
send_message(MSG_CURRENT_WAYPOINT);
|
||||||
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
|
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
|
||||||
send_message(MSG_NAV_CONTROLLER_OUTPUT);
|
send_message(MSG_NAV_CONTROLLER_OUTPUT);
|
||||||
//Serial.printf("mav2 %d\n", (int)streamRateExtendedStatus.get());
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) {
|
if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) {
|
||||||
// sent with GPS read
|
// sent with GPS read
|
||||||
send_message(MSG_LOCATION);
|
send_message(MSG_LOCATION);
|
||||||
//Serial.printf("mav3 %d\n", (int)streamRatePosition.get());
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) {
|
if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) {
|
||||||
// This is used for HIL. Do not change without discussing with HIL maintainers
|
// This is used for HIL. Do not change without discussing with HIL maintainers
|
||||||
send_message(MSG_SERVO_OUT);
|
send_message(MSG_SERVO_OUT);
|
||||||
//Serial.printf("mav4 %d\n", (int)streamRateRawController.get());
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) {
|
if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) {
|
||||||
send_message(MSG_RADIO_OUT);
|
send_message(MSG_RADIO_OUT);
|
||||||
send_message(MSG_RADIO_IN);
|
send_message(MSG_RADIO_IN);
|
||||||
//Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get());
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info
|
if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info
|
||||||
send_message(MSG_ATTITUDE);
|
send_message(MSG_ATTITUDE);
|
||||||
//Serial.printf("mav6 %d\n", (int)streamRateExtra1.get());
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info
|
if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info
|
||||||
send_message(MSG_VFR_HUD);
|
send_message(MSG_VFR_HUD);
|
||||||
//Serial.printf("mav7 %d\n", (int)streamRateExtra2.get());
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
|
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
|
||||||
// Available datastream
|
// Available datastream
|
||||||
//Serial.printf("mav8 %d\n", (int)streamRateExtra3.get());
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void
|
void
|
||||||
GCS_MAVLINK::send_message(enum ap_message id)
|
GCS_MAVLINK::send_message(enum ap_message id)
|
||||||
{
|
{
|
||||||
|
@ -430,7 +892,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
waypoint_receiving = false;
|
waypoint_receiving = false;
|
||||||
waypoint_dest_sysid = msg->sysid;
|
waypoint_dest_sysid = msg->sysid;
|
||||||
waypoint_dest_compid = msg->compid;
|
waypoint_dest_compid = msg->compid;
|
||||||
requested_interface = chan;
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -575,7 +1036,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
_queued_parameter = AP_Var::first();
|
_queued_parameter = AP_Var::first();
|
||||||
_queued_parameter_index = 0;
|
_queued_parameter_index = 0;
|
||||||
_queued_parameter_count = _count_parameters();
|
_queued_parameter_count = _count_parameters();
|
||||||
requested_interface = chan;
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1020,17 +1480,15 @@ GCS_MAVLINK::_find_parameter(uint16_t index)
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Send low-priority messages at a maximum rate of xx Hertz
|
* @brief Send the next pending parameter, called from deferred message
|
||||||
*
|
* handling code
|
||||||
* This function sends messages at a lower rate to not exceed the wireless
|
|
||||||
* bandwidth. It sends one message each time it is called until the buffer is empty.
|
|
||||||
* Call this function with xx Hertz to increase/decrease the bandwidth.
|
|
||||||
*/
|
*/
|
||||||
void
|
void
|
||||||
GCS_MAVLINK::_queued_send()
|
GCS_MAVLINK::queued_param_send()
|
||||||
{
|
{
|
||||||
// Check to see if we are sending parameters
|
// Check to see if we are sending parameters
|
||||||
if (NULL != _queued_parameter && (requested_interface == (unsigned)chan) && mavdelay > 1) {
|
if (NULL == _queued_parameter) return;
|
||||||
|
|
||||||
AP_Var *vp;
|
AP_Var *vp;
|
||||||
float value;
|
float value;
|
||||||
|
|
||||||
|
@ -1041,7 +1499,6 @@ GCS_MAVLINK::_queued_send()
|
||||||
// if the parameter can be cast to float, report it here and break out of the loop
|
// if the parameter can be cast to float, report it here and break out of the loop
|
||||||
value = vp->cast_to_float();
|
value = vp->cast_to_float();
|
||||||
if (!isnan(value)) {
|
if (!isnan(value)) {
|
||||||
|
|
||||||
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));
|
||||||
|
|
||||||
|
@ -1054,31 +1511,25 @@ GCS_MAVLINK::_queued_send()
|
||||||
|
|
||||||
_queued_parameter_index++;
|
_queued_parameter_index++;
|
||||||
}
|
}
|
||||||
mavdelay = 0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// this is called at 50hz, count runs to prevent flooding serialport and delayed to allow eeprom write
|
/**
|
||||||
mavdelay++;
|
* @brief Send the next pending waypoint, called from deferred message
|
||||||
|
* handling code
|
||||||
// request waypoints one by one
|
*/
|
||||||
// XXX note that this is pan-interface
|
void
|
||||||
|
GCS_MAVLINK::queued_waypoint_send()
|
||||||
|
{
|
||||||
if (waypoint_receiving &&
|
if (waypoint_receiving &&
|
||||||
(requested_interface == (unsigned)chan) &&
|
waypoint_request_i <= (unsigned)g.waypoint_total) {
|
||||||
waypoint_request_i <= (unsigned)g.waypoint_total &&
|
|
||||||
mavdelay > 15) { // limits to 3.33 hz
|
|
||||||
|
|
||||||
mavlink_msg_waypoint_request_send(
|
mavlink_msg_waypoint_request_send(
|
||||||
chan,
|
chan,
|
||||||
waypoint_dest_sysid,
|
waypoint_dest_sysid,
|
||||||
waypoint_dest_compid,
|
waypoint_dest_compid,
|
||||||
waypoint_request_i);
|
waypoint_request_i);
|
||||||
|
|
||||||
mavdelay = 0;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
a delay() callback that processes MAVLink packets. We set this as the
|
a delay() callback that processes MAVLink packets. We set this as the
|
||||||
callback in long running library initialisation routines to allow
|
callback in long running library initialisation routines to allow
|
||||||
|
|
|
@ -1,470 +0,0 @@
|
||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
||||||
|
|
||||||
#ifndef Mavlink_Common_H
|
|
||||||
#define Mavlink_Common_H
|
|
||||||
|
|
||||||
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK || GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
|
|
||||||
|
|
||||||
byte mavdelay = 0;
|
|
||||||
|
|
||||||
|
|
||||||
static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
|
|
||||||
{
|
|
||||||
//Serial.print("target = "); Serial.print(sysid, DEC); Serial.print("\tcomp = "); Serial.println(compid, DEC); //This line for debug only
|
|
||||||
if (sysid != mavlink_system.sysid){
|
|
||||||
return 1;
|
|
||||||
|
|
||||||
// Currently we are not checking for correct compid since APM is not passing mavlink info to any subsystem
|
|
||||||
// If it is addressed to our system ID we assume it is for us
|
|
||||||
//}else if(compid != mavlink_system.compid){
|
|
||||||
// gcs.send_text_P(SEVERITY_LOW,PSTR("component id mismatch"));
|
|
||||||
// return 1; // XXX currently not receiving correct compid from gcs
|
|
||||||
|
|
||||||
}else{
|
|
||||||
return 0; // no error
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
|
|
||||||
|
|
||||||
/*
|
|
||||||
!!NOTE!!
|
|
||||||
|
|
||||||
the use of NOINLINE separate functions for each message type avoids
|
|
||||||
a compiler bug in gcc that would cause it to use far more stack
|
|
||||||
space than is needed. Without the NOINLINE we use the sum of the
|
|
||||||
stack needed for each message type. Please be careful to follow the
|
|
||||||
pattern below when adding any new messages
|
|
||||||
*/
|
|
||||||
|
|
||||||
#define NOINLINE __attribute__((noinline))
|
|
||||||
|
|
||||||
static NOINLINE void send_heartbeat(mavlink_channel_t chan)
|
|
||||||
{
|
|
||||||
mavlink_msg_heartbeat_send(
|
|
||||||
chan,
|
|
||||||
mavlink_system.type,
|
|
||||||
MAV_AUTOPILOT_ARDUPILOTMEGA);
|
|
||||||
}
|
|
||||||
|
|
||||||
static NOINLINE void send_attitude(mavlink_channel_t chan)
|
|
||||||
{
|
|
||||||
Vector3f omega = dcm.get_gyro();
|
|
||||||
mavlink_msg_attitude_send(
|
|
||||||
chan,
|
|
||||||
micros(),
|
|
||||||
dcm.roll,
|
|
||||||
dcm.pitch,
|
|
||||||
dcm.yaw,
|
|
||||||
omega.x,
|
|
||||||
omega.y,
|
|
||||||
omega.z);
|
|
||||||
}
|
|
||||||
|
|
||||||
static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops)
|
|
||||||
{
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint8_t status = MAV_STATE_ACTIVE;
|
|
||||||
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
|
|
||||||
|
|
||||||
mavlink_msg_sys_status_send(
|
|
||||||
chan,
|
|
||||||
mode,
|
|
||||||
nav_mode,
|
|
||||||
status,
|
|
||||||
load * 1000,
|
|
||||||
battery_voltage * 1000,
|
|
||||||
battery_remaining,
|
|
||||||
packet_drops);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void NOINLINE send_meminfo(mavlink_channel_t chan)
|
|
||||||
{
|
|
||||||
extern unsigned __brkval;
|
|
||||||
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
|
|
||||||
}
|
|
||||||
|
|
||||||
static void NOINLINE send_location(mavlink_channel_t chan)
|
|
||||||
{
|
|
||||||
Matrix3f rot = dcm.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);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
|
|
||||||
{
|
|
||||||
mavlink_msg_nav_controller_output_send(
|
|
||||||
chan,
|
|
||||||
nav_roll / 1.0e2,
|
|
||||||
nav_pitch / 1.0e2,
|
|
||||||
nav_bearing / 1.0e2,
|
|
||||||
target_bearing / 1.0e2,
|
|
||||||
wp_distance,
|
|
||||||
altitude_error / 1.0e2,
|
|
||||||
airspeed_error,
|
|
||||||
crosstrack_error);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
|
|
||||||
{
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void NOINLINE send_servo_out(mavlink_channel_t chan)
|
|
||||||
{
|
|
||||||
const uint8_t rssi = 1;
|
|
||||||
// normalized values scaled to -10000 to 10000
|
|
||||||
// This is used for HIL. Do not change without discussing with HIL maintainers
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void NOINLINE send_radio_in(mavlink_channel_t chan)
|
|
||||||
{
|
|
||||||
uint8_t rssi = 1;
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void NOINLINE send_radio_out(mavlink_channel_t chan)
|
|
||||||
{
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
|
|
||||||
{
|
|
||||||
mavlink_msg_vfr_hud_send(
|
|
||||||
chan,
|
|
||||||
(float)airspeed / 100.0,
|
|
||||||
(float)g_gps->ground_speed / 100.0,
|
|
||||||
(dcm.yaw_sensor / 100) % 360,
|
|
||||||
(int)g.channel_throttle.servo_out,
|
|
||||||
current_loc.alt / 100.0,
|
|
||||||
climb_rate);
|
|
||||||
}
|
|
||||||
|
|
||||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
|
||||||
static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
|
|
||||||
{
|
|
||||||
Vector3f accel = imu.get_accel();
|
|
||||||
Vector3f gyro = imu.get_gyro();
|
|
||||||
|
|
||||||
mavlink_msg_raw_imu_send(
|
|
||||||
chan,
|
|
||||||
micros(),
|
|
||||||
accel.x * 1000.0 / gravity,
|
|
||||||
accel.y * 1000.0 / gravity,
|
|
||||||
accel.z * 1000.0 / gravity,
|
|
||||||
gyro.x * 1000.0,
|
|
||||||
gyro.y * 1000.0,
|
|
||||||
gyro.z * 1000.0,
|
|
||||||
compass.mag_x,
|
|
||||||
compass.mag_y,
|
|
||||||
compass.mag_z);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
|
|
||||||
{
|
|
||||||
mavlink_msg_scaled_pressure_send(
|
|
||||||
chan,
|
|
||||||
micros(),
|
|
||||||
(float)barometer.Press/100.0,
|
|
||||||
(float)(barometer.Press-g.ground_pressure)/100.0,
|
|
||||||
(int)(barometer.Temp*10));
|
|
||||||
}
|
|
||||||
|
|
||||||
static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
|
|
||||||
{
|
|
||||||
Vector3f mag_offsets = compass.get_offsets();
|
|
||||||
|
|
||||||
mavlink_msg_sensor_offsets_send(chan,
|
|
||||||
mag_offsets.x,
|
|
||||||
mag_offsets.y,
|
|
||||||
mag_offsets.z,
|
|
||||||
compass.get_declination(),
|
|
||||||
barometer.RawPress,
|
|
||||||
barometer.RawTemp,
|
|
||||||
imu.gx(), imu.gy(), imu.gz(),
|
|
||||||
imu.ax(), imu.ay(), imu.az());
|
|
||||||
}
|
|
||||||
#endif // HIL_MODE != HIL_MODE_ATTITUDE
|
|
||||||
|
|
||||||
static void NOINLINE send_gps_status(mavlink_channel_t chan)
|
|
||||||
{
|
|
||||||
mavlink_msg_gps_status_send(
|
|
||||||
chan,
|
|
||||||
g_gps->num_sats,
|
|
||||||
NULL,
|
|
||||||
NULL,
|
|
||||||
NULL,
|
|
||||||
NULL,
|
|
||||||
NULL);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
|
|
||||||
{
|
|
||||||
mavlink_msg_waypoint_current_send(
|
|
||||||
chan,
|
|
||||||
g.waypoint_index);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// try to send a message, return false if it won't fit in the serial tx buffer
|
|
||||||
static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
|
|
||||||
{
|
|
||||||
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
|
||||||
|
|
||||||
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
|
|
||||||
// defer any messages on the telemetry port for 1 second after
|
|
||||||
// bootup, to try to prevent bricking of Xbees
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
switch (id) {
|
|
||||||
case MSG_HEARTBEAT:
|
|
||||||
CHECK_PAYLOAD_SIZE(HEARTBEAT);
|
|
||||||
send_heartbeat(chan);
|
|
||||||
return true;
|
|
||||||
|
|
||||||
case MSG_EXTENDED_STATUS1:
|
|
||||||
CHECK_PAYLOAD_SIZE(SYS_STATUS);
|
|
||||||
send_extended_status1(chan, packet_drops);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MSG_EXTENDED_STATUS2:
|
|
||||||
CHECK_PAYLOAD_SIZE(MEMINFO);
|
|
||||||
send_meminfo(chan);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MSG_ATTITUDE:
|
|
||||||
CHECK_PAYLOAD_SIZE(ATTITUDE);
|
|
||||||
send_attitude(chan);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MSG_LOCATION:
|
|
||||||
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
|
|
||||||
send_location(chan);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MSG_NAV_CONTROLLER_OUTPUT:
|
|
||||||
if (control_mode != MANUAL) {
|
|
||||||
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
|
|
||||||
send_nav_controller_output(chan);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MSG_GPS_RAW:
|
|
||||||
CHECK_PAYLOAD_SIZE(GPS_RAW);
|
|
||||||
send_gps_raw(chan);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MSG_SERVO_OUT:
|
|
||||||
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
|
|
||||||
send_servo_out(chan);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MSG_RADIO_IN:
|
|
||||||
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
|
|
||||||
send_radio_in(chan);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MSG_RADIO_OUT:
|
|
||||||
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
|
|
||||||
send_radio_out(chan);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MSG_VFR_HUD:
|
|
||||||
CHECK_PAYLOAD_SIZE(VFR_HUD);
|
|
||||||
send_vfr_hud(chan);
|
|
||||||
break;
|
|
||||||
|
|
||||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
|
||||||
case MSG_RAW_IMU1:
|
|
||||||
CHECK_PAYLOAD_SIZE(RAW_IMU);
|
|
||||||
send_raw_imu1(chan);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MSG_RAW_IMU2:
|
|
||||||
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
|
|
||||||
send_raw_imu2(chan);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MSG_RAW_IMU3:
|
|
||||||
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
|
|
||||||
send_raw_imu3(chan);
|
|
||||||
break;
|
|
||||||
#endif // HIL_MODE != HIL_MODE_ATTITUDE
|
|
||||||
|
|
||||||
case MSG_GPS_STATUS:
|
|
||||||
CHECK_PAYLOAD_SIZE(GPS_STATUS);
|
|
||||||
send_gps_status(chan);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MSG_CURRENT_WAYPOINT:
|
|
||||||
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
|
|
||||||
send_current_waypoint(chan);
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED
|
|
||||||
static struct mavlink_queue {
|
|
||||||
enum ap_message deferred_messages[MAX_DEFERRED_MESSAGES];
|
|
||||||
uint8_t next_deferred_message;
|
|
||||||
uint8_t num_deferred_messages;
|
|
||||||
} mavlink_queue[2];
|
|
||||||
|
|
||||||
// send a message using mavlink
|
|
||||||
static void mavlink_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops)
|
|
||||||
{
|
|
||||||
uint8_t i, nextid;
|
|
||||||
struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan];
|
|
||||||
|
|
||||||
// see if we can send the deferred messages, if any
|
|
||||||
while (q->num_deferred_messages != 0) {
|
|
||||||
if (!mavlink_try_send_message(chan,
|
|
||||||
q->deferred_messages[q->next_deferred_message],
|
|
||||||
packet_drops)) {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
q->next_deferred_message++;
|
|
||||||
if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) {
|
|
||||||
q->next_deferred_message = 0;
|
|
||||||
}
|
|
||||||
q->num_deferred_messages--;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (id == MSG_RETRY_DEFERRED) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// this message id might already be deferred
|
|
||||||
for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) {
|
|
||||||
if (q->deferred_messages[nextid] == id) {
|
|
||||||
// its already deferred, discard
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
nextid++;
|
|
||||||
if (nextid == MAX_DEFERRED_MESSAGES) {
|
|
||||||
nextid = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (q->num_deferred_messages != 0 ||
|
|
||||||
!mavlink_try_send_message(chan, id, packet_drops)) {
|
|
||||||
// can't send it now, so defer it
|
|
||||||
if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) {
|
|
||||||
// the defer buffer is full, discard
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
nextid = q->next_deferred_message + q->num_deferred_messages;
|
|
||||||
if (nextid >= MAX_DEFERRED_MESSAGES) {
|
|
||||||
nextid -= MAX_DEFERRED_MESSAGES;
|
|
||||||
}
|
|
||||||
q->deferred_messages[nextid] = id;
|
|
||||||
q->num_deferred_messages++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str)
|
|
||||||
{
|
|
||||||
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
|
|
||||||
// don't send status MAVLink messages for 1 second after
|
|
||||||
// bootup, to try to prevent Xbee bricking
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
mavlink_msg_statustext_send(
|
|
||||||
chan,
|
|
||||||
severity,
|
|
||||||
(const int8_t*) str);
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // mavlink in use
|
|
||||||
|
|
||||||
#endif // inclusion guard
|
|
|
@ -126,6 +126,8 @@ enum ap_message {
|
||||||
MSG_GPS_STATUS,
|
MSG_GPS_STATUS,
|
||||||
MSG_GPS_RAW,
|
MSG_GPS_RAW,
|
||||||
MSG_SERVO_OUT,
|
MSG_SERVO_OUT,
|
||||||
|
MSG_NEXT_WAYPOINT,
|
||||||
|
MSG_NEXT_PARAM,
|
||||||
MSG_RETRY_DEFERRED // this must be last
|
MSG_RETRY_DEFERRED // this must be last
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -229,4 +231,7 @@ enum ap_message {
|
||||||
// convert a boolean (0 or 1) to a sign for multiplying (0 maps to 1, 1 maps to -1)
|
// convert a boolean (0 or 1) to a sign for multiplying (0 maps to 1, 1 maps to -1)
|
||||||
#define BOOL_TO_SIGN(bvalue) ((bvalue)?-1:1)
|
#define BOOL_TO_SIGN(bvalue) ((bvalue)?-1:1)
|
||||||
|
|
||||||
|
// mark a function as not to be inlined
|
||||||
|
#define NOINLINE __attribute__((noinline))
|
||||||
|
|
||||||
#endif // _DEFINES_H
|
#endif // _DEFINES_H
|
||||||
|
|
Loading…
Reference in New Issue