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

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:
Andrew Tridgell 2011-09-18 16:39:09 +10:00
parent ef6e2c2adf
commit 225e6d760f
4 changed files with 518 additions and 531 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduPlane/GCS.h
View File

@ -131,12 +131,14 @@ public:
void send_text(uint8_t severity, const char *str);
void send_text(uint8_t severity, const prog_char_t *str);
void data_stream_send(uint16_t freqMin, uint16_t freqMax);
void queued_param_send();
void queued_waypoint_send();
private:
void handleMessage(mavlink_message_t * msg);
/// Perform queued sending operations
///
void _queued_send();
AP_Var *_queued_parameter; ///< next parameter to be sent in queue
uint16_t _queued_parameter_index; ///< next queued parameter's index
@ -160,7 +162,6 @@ private:
uint16_t packet_drops;
// waypoints
uint16_t requested_interface; // request port to use
uint16_t waypoint_request_i; // request index
uint16_t waypoint_dest_sysid; // where to send requests
uint16_t waypoint_dest_compid; // "

537
ArduPlane/GCS_Mavlink.pde
View File

@ -1,12 +1,475 @@
// -*- 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
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) :
packet_drops(0),
@ -35,7 +498,7 @@ void
GCS_MAVLINK::init(FastSerial * port)
{
GCS_Class::init(port);
if (port == &Serial) { // to split hil vs gcs
if (port == &Serial) {
mavlink_comm_0_port = port;
chan = MAVLINK_COMM_0;
}else{
@ -54,29 +517,34 @@ GCS_MAVLINK::update(void)
status.packet_rx_drop_count = 0;
// process received bytes
while(comm_get_available(chan))
while (comm_get_available(chan))
{
uint8_t c = comm_receive_ch(chan);
// Try to get a new message
if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg);
if (mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg);
}
// Update packet drops counter
packet_drops += status.packet_rx_drop_count;
// 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
if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){
send_text_P(SEVERITY_LOW,PSTR("waypoint send timeout"));
waypoint_sending = false;
}
// stop waypoint receiving if timeout
if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){
send_text_P(SEVERITY_LOW,PSTR("waypoint receive timeout"));
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_IMU2);
send_message(MSG_RAW_IMU3);
//Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get());
}
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_GPS_RAW); // TODO - remove this message after location message is working
send_message(MSG_NAV_CONTROLLER_OUTPUT);
//Serial.printf("mav2 %d\n", (int)streamRateExtendedStatus.get());
}
if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) {
// sent with GPS read
send_message(MSG_LOCATION);
//Serial.printf("mav3 %d\n", (int)streamRatePosition.get());
}
if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) {
// This is used for HIL. Do not change without discussing with HIL maintainers
send_message(MSG_SERVO_OUT);
//Serial.printf("mav4 %d\n", (int)streamRateRawController.get());
}
if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) {
send_message(MSG_RADIO_OUT);
send_message(MSG_RADIO_IN);
//Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get());
}
if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info
send_message(MSG_ATTITUDE);
//Serial.printf("mav6 %d\n", (int)streamRateExtra1.get());
}
if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info
send_message(MSG_VFR_HUD);
//Serial.printf("mav7 %d\n", (int)streamRateExtra2.get());
}
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
// Available datastream
//Serial.printf("mav8 %d\n", (int)streamRateExtra3.get());
}
}
}
void
GCS_MAVLINK::send_message(enum ap_message id)
{
@ -430,7 +892,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
waypoint_receiving = false;
waypoint_dest_sysid = msg->sysid;
waypoint_dest_compid = msg->compid;
requested_interface = chan;
break;
}
@ -575,7 +1036,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
_queued_parameter = AP_Var::first();
_queued_parameter_index = 0;
_queued_parameter_count = _count_parameters();
requested_interface = chan;
break;
}
@ -1020,17 +1480,15 @@ GCS_MAVLINK::_find_parameter(uint16_t index)
}
/**
* @brief Send low-priority messages at a maximum rate of xx Hertz
*
* 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.
* @brief Send the next pending parameter, called from deferred message
* handling code
*/
void
GCS_MAVLINK::_queued_send()
GCS_MAVLINK::queued_param_send()
{
// 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;
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
value = vp->cast_to_float();
if (!isnan(value)) {
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
vp->copy_name(param_name, sizeof(param_name));
@ -1054,31 +1511,25 @@ GCS_MAVLINK::_queued_send()
_queued_parameter_index++;
}
mavdelay = 0;
}
}
// this is called at 50hz, count runs to prevent flooding serialport and delayed to allow eeprom write
mavdelay++;
// request waypoints one by one
// XXX note that this is pan-interface
/**
* @brief Send the next pending waypoint, called from deferred message
* handling code
*/
void
GCS_MAVLINK::queued_waypoint_send()
{
if (waypoint_receiving &&
(requested_interface == (unsigned)chan) &&
waypoint_request_i <= (unsigned)g.waypoint_total &&
mavdelay > 15) { // limits to 3.33 hz
waypoint_request_i <= (unsigned)g.waypoint_total) {
mavlink_msg_waypoint_request_send(
chan,
waypoint_dest_sysid,
waypoint_dest_compid,
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
callback in long running library initialisation routines to allow

470
ArduPlane/Mavlink_Common.h
View File

@ -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

5
ArduPlane/defines.h
View File

@ -126,6 +126,8 @@ enum ap_message {
MSG_GPS_STATUS,
MSG_GPS_RAW,
MSG_SERVO_OUT,
MSG_NEXT_WAYPOINT,
MSG_NEXT_PARAM,
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)
#define BOOL_TO_SIGN(bvalue) ((bvalue)?-1:1)
// mark a function as not to be inlined
#define NOINLINE __attribute__((noinline))
#endif // _DEFINES_H