|
|
|
@ -11,19 +11,19 @@ static mavlink_statustext_t pending_status;
@@ -11,19 +11,19 @@ static mavlink_statustext_t pending_status;
|
|
|
|
|
static bool mavlink_active; |
|
|
|
|
|
|
|
|
|
// 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 |
|
|
|
|
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_ ## id ## _LEN) return false |
|
|
|
|
|
|
|
|
|
// prototype this for use inside the GCS class |
|
|
|
|
void gcs_send_text_fmt(const prog_char_t *fmt, ...); |
|
|
|
|
|
|
|
|
|
/* |
|
|
|
|
!!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 |
|
|
|
|
* !!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) |
|
|
|
@ -206,8 +206,8 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
@@ -206,8 +206,8 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
|
|
|
|
|
|
|
|
|
|
if (g.battery_monitoring == 3) { |
|
|
|
|
/*setting a out-of-range value. |
|
|
|
|
It informs to external devices that |
|
|
|
|
it cannot be calculated properly just by voltage*/ |
|
|
|
|
* It informs to external devices that |
|
|
|
|
* it cannot be calculated properly just by voltage*/ |
|
|
|
|
battery_remaining = 150; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -250,7 +250,7 @@ static void NOINLINE send_location(mavlink_channel_t chan)
@@ -250,7 +250,7 @@ static void NOINLINE send_location(mavlink_channel_t chan)
|
|
|
|
|
|
|
|
|
|
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) |
|
|
|
|
{ |
|
|
|
|
int16_t bearing = (hold_course==-1?nav_bearing_cd:hold_course) / 100; |
|
|
|
|
int16_t bearing = (hold_course==-1 ? nav_bearing_cd : hold_course) / 100; |
|
|
|
|
mavlink_msg_nav_controller_output_send( |
|
|
|
|
chan, |
|
|
|
|
nav_roll_cd * 0.01, |
|
|
|
@ -756,7 +756,7 @@ GCS_MAVLINK::update(void)
@@ -756,7 +756,7 @@ GCS_MAVLINK::update(void)
|
|
|
|
|
|
|
|
|
|
#if CLI_ENABLED == ENABLED |
|
|
|
|
/* allow CLI to be started by hitting enter 3 times, if no |
|
|
|
|
heartbeat packets have been received */ |
|
|
|
|
* heartbeat packets have been received */ |
|
|
|
|
if (mavlink_active == 0 && millis() < 20000) { |
|
|
|
|
if (c == '\n' || c == '\r') { |
|
|
|
|
crlf_count++; |
|
|
|
@ -793,12 +793,12 @@ GCS_MAVLINK::update(void)
@@ -793,12 +793,12 @@ GCS_MAVLINK::update(void)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// 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) { |
|
|
|
|
waypoint_sending = false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// 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) { |
|
|
|
|
waypoint_receiving = false; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -955,7 +955,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -955,7 +955,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
else |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
switch(packet.req_stream_id){ |
|
|
|
|
switch(packet.req_stream_id) { |
|
|
|
|
|
|
|
|
|
case MAV_DATA_STREAM_ALL: |
|
|
|
|
streamRateRawSensors.set_and_save_ifchanged(freq); |
|
|
|
@ -1148,7 +1148,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1148,7 +1148,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
frame = MAV_FRAME_GLOBAL; // reference frame |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
float param1 = 0, param2 = 0 , param3 = 0, param4 = 0; |
|
|
|
|
float param1 = 0, param2 = 0, param3 = 0, param4 = 0; |
|
|
|
|
|
|
|
|
|
// time that the mav should loiter in milliseconds |
|
|
|
|
uint8_t current = 0; // 1 (true), 0 (false) |
|
|
|
@ -1494,11 +1494,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1494,11 +1494,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
|
|
|
|
|
if (result != MAV_MISSION_ACCEPTED) goto mission_failed; |
|
|
|
|
|
|
|
|
|
if(packet.current == 2){ //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission |
|
|
|
|
if(packet.current == 2) { //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission |
|
|
|
|
guided_WP = tell_command; |
|
|
|
|
|
|
|
|
|
// add home alt if needed |
|
|
|
|
if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT){ |
|
|
|
|
if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT) { |
|
|
|
|
guided_WP.alt += home.alt; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -1549,7 +1549,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1549,7 +1549,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
mission_failed: |
|
|
|
|
mission_failed: |
|
|
|
|
// we are rejecting the mission/waypoint |
|
|
|
|
mavlink_msg_mission_ack_send( |
|
|
|
|
chan, |
|
|
|
@ -1907,9 +1907,9 @@ GCS_MAVLINK::_count_parameters()
@@ -1907,9 +1907,9 @@ GCS_MAVLINK::_count_parameters()
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/** |
|
|
|
|
* @brief Send the next pending parameter, called from deferred message |
|
|
|
|
* handling code |
|
|
|
|
*/ |
|
|
|
|
* @brief Send the next pending parameter, called from deferred message |
|
|
|
|
* handling code |
|
|
|
|
*/ |
|
|
|
|
void |
|
|
|
|
GCS_MAVLINK::queued_param_send() |
|
|
|
|
{ |
|
|
|
@ -1941,9 +1941,9 @@ GCS_MAVLINK::queued_param_send()
@@ -1941,9 +1941,9 @@ GCS_MAVLINK::queued_param_send()
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/** |
|
|
|
|
* @brief Send the next pending waypoint, called from deferred message |
|
|
|
|
* handling code |
|
|
|
|
*/ |
|
|
|
|
* @brief Send the next pending waypoint, called from deferred message |
|
|
|
|
* handling code |
|
|
|
|
*/ |
|
|
|
|
void |
|
|
|
|
GCS_MAVLINK::queued_waypoint_send() |
|
|
|
|
{ |
|
|
|
@ -1958,11 +1958,11 @@ GCS_MAVLINK::queued_waypoint_send()
@@ -1958,11 +1958,11 @@ GCS_MAVLINK::queued_waypoint_send()
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* |
|
|
|
|
a delay() callback that processes MAVLink packets. We set this as the |
|
|
|
|
callback in long running library initialisation routines to allow |
|
|
|
|
MAVLink to process packets while waiting for the initialisation to |
|
|
|
|
complete |
|
|
|
|
*/ |
|
|
|
|
* a delay() callback that processes MAVLink packets. We set this as the |
|
|
|
|
* callback in long running library initialisation routines to allow |
|
|
|
|
* MAVLink to process packets while waiting for the initialisation to |
|
|
|
|
* complete |
|
|
|
|
*/ |
|
|
|
|
static void mavlink_delay(unsigned long t) |
|
|
|
|
{ |
|
|
|
|
uint32_t tstart; |
|
|
|
@ -2004,7 +2004,7 @@ static void mavlink_delay(unsigned long t)
@@ -2004,7 +2004,7 @@ static void mavlink_delay(unsigned long t)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* |
|
|
|
|
send a message on both GCS links |
|
|
|
|
* send a message on both GCS links |
|
|
|
|
*/ |
|
|
|
|
static void gcs_send_message(enum ap_message id) |
|
|
|
|
{ |
|
|
|
@ -2015,7 +2015,7 @@ static void gcs_send_message(enum ap_message id)
@@ -2015,7 +2015,7 @@ static void gcs_send_message(enum ap_message id)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* |
|
|
|
|
send data streams in the given rate range on both links |
|
|
|
|
* send data streams in the given rate range on both links |
|
|
|
|
*/ |
|
|
|
|
static void gcs_data_stream_send(void) |
|
|
|
|
{ |
|
|
|
@ -2026,7 +2026,7 @@ static void gcs_data_stream_send(void)
@@ -2026,7 +2026,7 @@ static void gcs_data_stream_send(void)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* |
|
|
|
|
look for incoming commands on the GCS links |
|
|
|
|
* look for incoming commands on the GCS links |
|
|
|
|
*/ |
|
|
|
|
static void gcs_update(void) |
|
|
|
|
{ |
|
|
|
@ -2045,9 +2045,9 @@ static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
@@ -2045,9 +2045,9 @@ static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* |
|
|
|
|
send a low priority formatted message to the GCS |
|
|
|
|
only one fits in the queue, so if you send more than one before the |
|
|
|
|
last one gets into the serial buffer then the old one will be lost |
|
|
|
|
* send a low priority formatted message to the GCS |
|
|
|
|
* only one fits in the queue, so if you send more than one before the |
|
|
|
|
* last one gets into the serial buffer then the old one will be lost |
|
|
|
|
*/ |
|
|
|
|
void gcs_send_text_fmt(const prog_char_t *fmt, ...) |
|
|
|
|
{ |
|
|
|
|