/**
* @brief Send one of the buffered messages
*/
void debug_message_send_one(void)
{
if (m_debug_index_write == m_debug_index_read)
{
//buffer empty
return;
}
m_debug_index_read = (m_debug_index_read + 1) % DEBUG_COUNT;
char msg[DEBUG_MAX_LEN];
// use pointer to program code
snprintf(msg, DEBUG_MAX_LEN, m_debug_buf_pointer[m_debug_index_read],
m_debug_buf_int[m_debug_index_read]);
// strncpy(msg, m_debug_buf_pointer[m_debug_index_read], DEBUG_MAX_LEN);
// use copied string from buffer
// snprintf(msg, DEBUG_MAX_LEN, m_debug_buf[m_debug_index_read],
// m_debug_buf_int[m_debug_index_read]);
// strncpy(msg, m_debug_buf[m_debug_index_read], DEBUG_MAX_LEN);
if(m_debug_was_full){
msg[0] = '+';
m_debug_was_full=0;
}
msg[DEBUG_MAX_LEN - 1] = '\0';//enforce string termination
mavlink_msg_statustext_send(MAVLINK_COMM_0, 0, (int8_t*) msg);
mavlink_msg_statustext_send(MAVLINK_COMM_1, 0, (int8_t*) msg);
}
void Tracker::send_statustext(mavlink_channel_t chan)
{
mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status;
mavlink_msg_statustext_send(
chan,
s->severity,
s->text);
}
void mav_printf(const char* format, ...)
{
char buf[200];
va_list arglist;
va_start(arglist, format);
vsnprintf(buf, sizeof(buf), format, arglist);
// mavlink_msg_statustext_send(MAVLINK_COMM_1, severity, text);
// severity: Severity of status, 0 = info message, 255 = critical fault (uint8_t)
mavlink_msg_statustext_send(MAVLINK_COMM_0, 0, buf);
va_end(arglist);
}
/*
send a statustext message to all active MAVLink connections
*/
void GCS_MAVLINK::send_statustext_all(const prog_char_t *msg)
{
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if ((1U<<i) & mavlink_active) {
mavlink_channel_t chan = (mavlink_channel_t)i;
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
char msg2[50];
strncpy_P(msg2, msg, sizeof(msg2));
mavlink_msg_statustext_send(chan,
SEVERITY_HIGH,
msg2);
#else
mavlink_msg_statustext_send(chan,
SEVERITY_HIGH,
msg);
#endif
}
}
}
}
void
GCS_MAVLINK::send_text(gcs_severity severity, const char *str)
{
if (severity == SEVERITY_LOW) {
// send via the deferred queuing system
mavlink_statustext_t *s = &pending_status;
s->severity = (uint8_t)severity;
strncpy((char *)s->text, str, sizeof(s->text));
send_message(MSG_STATUSTEXT);
} else {
// send immediately
mavlink_msg_statustext_send(chan, severity, str);
}
}
/**
* @warning This method is NOT interrupt service routine (ISR) safe!
* @param chan MAVLink channel to use
* @param text The string to send
*/
void debug_message_send_immediate(mavlink_channel_t chan, const char* text)
{
const int len = 50;
char str[len];
int i = 0;
while (i < len - 1)
{
str[i] = text[i];
if (text[i] == '\0')
break;
i++;
}
str[i] = '\0'; // Enforce null termination
mavlink_msg_statustext_send(chan, 0, (int8_t*) str);
}
bool try_send_statustext(mavlink_channel_t chan, const char *text, int len) {
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
CHECK_PAYLOAD_SIZE(STATUSTEXT);
char statustext[50] = { 0 };
if (len < 50) {
memcpy(statustext, text, len);
}
mavlink_msg_statustext_send(
chan,
1, /* SEVERITY_LOW */
statustext);
return true;
}
void AP_InertialSensor_UserInteract_MAVLink::_printf_P(const prog_char* fmt, ...)
{
char msg[50];
va_list ap;
va_start(ap, fmt);
hal.util->vsnprintf_P(msg, sizeof(msg), (const prog_char_t *)fmt, ap);
va_end(ap);
if (msg[strlen(msg)-1] == '\n') {
// STATUSTEXT messages should not add linefeed
msg[strlen(msg)-1] = 0;
}
while (comm_get_txspace(_chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES < sizeof(mavlink_statustext_t)) {
hal.scheduler->delay(1);
}
mavlink_msg_statustext_send(_chan, SEVERITY_USER_RESPONSE, msg);
}
void
GCS_MAVLINK::send_text(gcs_severity severity, const char *str)
{
if (severity != SEVERITY_LOW &&
comm_get_txspace(chan) >=
MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_STATUSTEXT_LEN) {
// send immediately
mavlink_msg_statustext_send(chan, severity, str);
} else {
// send via the deferred queuing system
mavlink_statustext_t *s = &pending_status;
s->severity = (uint8_t)severity;
strncpy((char *)s->text, str, sizeof(s->text));
send_message(MSG_STATUSTEXT);
}
}
/*
send a statustext message to all active MAVLink connections
*/
void GCS_MAVLINK::send_statustext_all(MAV_SEVERITY severity, const char *fmt, ...)
{
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
if ((1U<<i) & mavlink_active) {
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
char msg2[50] {};
va_list arg_list;
va_start(arg_list, fmt);
hal.util->vsnprintf((char *)msg2, sizeof(msg2), fmt, arg_list);
va_end(arg_list);
mavlink_msg_statustext_send(chan,
severity,
msg2);
}
}
}
}
int
Mavlink::send_statustext(unsigned severity, const char *string)
{
const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
mavlink_statustext_t statustext;
int i = 0;
while (i < len - 1) {
statustext.text[i] = string[i];
if (string[i] == '\0') {
break;
}
i++;
}
if (i > 1) {
/* Enforce null termination */
statustext.text[i] = '\0';
/* Map severity */
switch (severity) {
case MAVLINK_IOC_SEND_TEXT_INFO:
statustext.severity = MAV_SEVERITY_INFO;
break;
case MAVLINK_IOC_SEND_TEXT_CRITICAL:
statustext.severity = MAV_SEVERITY_CRITICAL;
break;
case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
statustext.severity = MAV_SEVERITY_EMERGENCY;
break;
}
mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
return OK;
} else {
return ERROR;
}
}
/*
send a statustext message to specific MAVLink connections in a bitmask
*/
void GCS_MAVLINK::service_statustext(void)
{
// create bitmask of what mavlink ports we should send this text to.
// note, if sending to all ports, we only need to store the bitmask for each and the string only once.
// once we send over a link, clear the port but other busy ports bit may stay allowing for faster links
// to clear the bit and send quickly but slower links to still store the string. Regardless of mixed
// bitrates of ports, a maximum of GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY strings can be buffered. Downside
// is if you have a super slow link mixed with a faster port, if there are GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY
// strings in the slow queue then the next item can not be queued for the faster link
if (_statustext_queue.empty()) {
// nothing to do
return;
}
for (uint8_t idx=0; idx<GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY; ) {
statustext_t *statustext = _statustext_queue[idx];
if (statustext == nullptr) {
break;
}
// try and send to all active mavlink ports listed in the statustext.bitmask
for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
uint8_t chan_bit = (1U<<i);
// logical AND (&) to mask them together
if (statustext->bitmask & chan_bit) {
// something is queued on a port and that's the port index we're looped at
mavlink_channel_t chan_index = (mavlink_channel_t)(MAVLINK_COMM_0+i);
if (comm_get_txspace(chan_index) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
// we have space so send then clear that channel bit on the mask
mavlink_msg_statustext_send(chan_index, statustext->msg.severity, statustext->msg.text);
statustext->bitmask &= ~chan_bit;
}
}
}
if (statustext->bitmask == 0) {
_statustext_queue.remove(idx);
} else {
// move to next index
idx++;
}
}
}
void mavlink_send_text(mavlink_channel_t chan, enum gcs_severity severity, char *str)
{
if (telemetry_delayed(chan)) {
return;
}
if (severity == SEVERITY_LOW) {
// send via the deferred queuing system
pending_status.severity = (uint8_t)severity;
mav_array_memcpy((char *)pending_status.text, str, sizeof(pending_status.text));
mavlink_send_message(chan, MSG_STATUSTEXT, 0);
} else {
// send immediately
mavlink_msg_statustext_send(
chan,
severity,
str);
}
}
void MavlinkComm::sendText(uint8_t severity, const char *str) {
mavlink_msg_statustext_send(_channel, severity, (const int8_t*) str);
}
void
GCS_MAVLINK::send_text(MAV_SEVERITY severity, const char *str)
{
if (severity < MAV_SEVERITY_WARNING &&
comm_get_txspace(chan) >=
MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_STATUSTEXT_LEN) {
// send immediately
char msg[50] {};
strncpy(msg, str, sizeof(msg));
mavlink_msg_statustext_send(chan, severity, msg);
} else {
// send via the deferred queuing system
mavlink_statustext_t *s = &pending_status;
s->severity = (uint8_t)severity;
strncpy((char *)s->text, str, sizeof(s->text));
send_message(MSG_STATUSTEXT);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN && APM_BUILD_TYPE(APM_BUILD_ArduCopter)
if (!strcmp(str, "compass disabled")) // Messages defined in compassmot.pde (10x)
textId = 1;
else if (!strcmp(str, "check compass"))
textId = 2;
else if (!strcmp(str, "RC not calibrated"))
textId = 3;
else if (!strcmp(str, "thr not zero"))
textId = 4;
else if (!strcmp(str, "Not landed"))
textId = 5;
else if (!strcmp(str, "STARTING CALIBRATION"))
textId = 6;
else if (!strcmp(str, "CURRENT"))
textId = 7;
else if (!strcmp(str, "THROTTLE"))
textId = 8;
else if (!strcmp(str, "Calibration Successful"))
textId = 9;
else if (!strcmp(str, "Failed"))
textId = 10;
else if (!strcmp(str, "AutoTune: Started")) // Messages defined in control_autotune.pde (4x)
textId = 21;
else if (!strcmp(str, "AutoTune: Stopped"))
textId = 22;
else if (!strcmp(str, "AutoTune: Success"))
textId = 23;
else if (!strcmp(str, "AutoTune: Failed"))
textId = 24;
else if (!strcmp(str, "Crash: Disarming")) // Messages defined in crash_check.pde (3x)
textId = 35;
else if (!strcmp(str, "Parachute: Released"))
textId = 36;
else if (!strcmp(str, "Parachute: Too Low"))
textId = 37;
else if (!strcmp(str, "EKF variance")) // Messages defined in ekf_check.pde (2x)
textId = 48;
else if (!strcmp(str, "DCM bad heading"))
textId = 49;
else if (!strcmp(str, "Low Battery")) // Messages defined in events.pde (2x)
textId = 60;
else if (!strcmp(str, "Lost GPS"))
textId = 61;
else if (!strcmp(str, "bad rally point message ID")) // Messages defined in GCS_Mavlink.pde (6x)
textId = 72;
else if (!strcmp(str, "bad rally point message count"))
textId = 73;
else if (!strcmp(str, "error setting rally point"))
textId = 74;
else if (!strcmp(str, "bad rally point index"))
textId = 75;
else if (!strcmp(str, "failed to set rally point"))
textId = 76;
else if (!strcmp(str, "Initialising APM..."))
textId = 77;
else if (!strcmp(str, "Erasing logs")) // Messages defined in Log.pde (2x)
textId = 88;
else if (!strcmp(str, "Log erase complete"))
textId = 89;
else if (!strcmp(str, "ARMING MOTORS")) // Messages defined in motors.pde (30x)
textId = 100;
else if (!strcmp(str, "Arm: Gyro cal failed"))
textId = 101;
else if (!strcmp(str, "PreArm: RC not calibrated"))
textId = 102;
else if (!strcmp(str, "PreArm: Baro not healthy"))
textId = 103;
else if (!strcmp(str, "PreArm: Alt disparity"))
textId = 104;
else if (!strcmp(str, "PreArm: Compass not healthy"))
textId = 105;
else if (!strcmp(str, "PreArm: Compass not calibrated"))
textId = 106;
else if (!strcmp(str, "PreArm: Compass offsets too high"))
textId = 107;
else if (!strcmp(str, "PreArm: Check mag field"))
textId = 108;
else if (!strcmp(str, "PreArm: compasses inconsistent"))
textId = 109;
else if (!strcmp(str, "PreArm: INS not calibrated"))
textId = 110;
else if (!strcmp(str, "PreArm: Accels not healthy"))
textId = 111;
else if (!strcmp(str, "PreArm: Accels inconsistent"))
textId = 112;
else if (!strcmp(str, "PreArm: Gyros not healthy"))
textId = 113;
else if (!strcmp(str, "PreArm: Gyro cal failed"))
textId = 114;
else if (!strcmp(str, "PreArm: Gyros inconsistent"))
textId = 115;
else if (!strcmp(str, "PreArm: Check Board Voltage"))
textId = 116;
else if (!strcmp(str, "PreArm: Ch7&Ch8 Opt cannot be same"))
textId = 117;
else if (!strcmp(str, "PreArm: Check FS_THR_VALUE"))
textId = 118;
else if (!strcmp(str, "PreArm: Check ANGLE_MAX"))
textId = 119;
else if (!strcmp(str, "PreArm: ACRO_BAL_ROLL/PITCH"))
textId = 120;
else if (!strcmp(str, "PreArm: GPS Glitch"))
textId = 121;
else if (!strcmp(str, "PreArm: Need 3D Fix"))
textId = 122;
else if (!strcmp(str, "PreArm: Bad Velocity"))
textId = 123;
else if (!strcmp(str, "PreArm: High GPS HDOP"))
textId = 124;
else if (!strcmp(str, "Arm: Alt disparity"))
textId = 125;
else if (!strcmp(str, "Arm: Thr below FS"))
textId = 126;
else if (!strcmp(str, "Arm: Leaning"))
textId = 127;
else if (!strcmp(str, "Arm: Safety Switch"))
textId = 128;
else if (!strcmp(str, "DISARMING MOTORS"))
textId = 129;
else if (!strcmp(str, "Motor Test: RC not calibrated")) // Messages defined in motor_test.pde (3x)
textId = 140;
else if (!strcmp(str, "Motor Test: vehicle not landed"))
textId = 141;
else if (!strcmp(str, "Motor Test: Safety Switch"))
textId = 142;
else if (!strcmp(str, "Calibrating barometer")) // Messages defined in sensors.pde (2x)
textId = 153;
else if (!strcmp(str, "barometer calibration complete"))
textId = 154;
else if (!strcmp(str, "Trim saved")) // Messages defined in switches.pde (1x)
textId = 165;
else if (!strcmp(str, "No dataflash inserted")) // Messages defined in system.pde (4x)
textId = 176;
else if (!strcmp(str, "ERASING LOGS"))
textId = 177;
else if (!strcmp(str, "Waiting for first HIL_STATE message"))
textId = 178;
else if (!strcmp(str, "GROUND START"))
textId = 179;
else if (!strcmp(str, "PreArm: Baro not healthy")) // Messages defined in AP_Arming.cpp (9x)
textId = 190;
else if (!strcmp(str, "PreArm: Compass not healthy"))
textId = 191;
else if (!strcmp(str, "PreArm: Compass not calibrated"))
textId = 192;
else if (!strcmp(str, "PreArm: Bad GPS Pos"))
textId = 193;
else if (!strcmp(str, "PreArm: Battery failsafe on"))
textId = 194;
else if (!strcmp(str, "PreArm: Hardware Safety Switch"))
textId = 195;
else if (!strcmp(str, "PreArm: Radio failsafe on"))
textId = 196;
else if (!strcmp(str, "Throttle armed"))
textId = 197;
else if (!strcmp(str, "Throttle disarmed"))
textId = 198;
else if (!strcmp(str, "flight plan update rejected")) // Messages defined in GCS_Common.cpp (2x)
textId = 209;
else if (!strcmp(str, "flight plan received"))
textId = 210;
else
textId = 0;
g_severity = severity;
#endif
}
/****************************************************************************
* Public Functions
****************************************************************************/
void handleMessage(mavlink_message_t * msg) {
//check for terminate command
if(msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG)
{
mavlink_command_long_t cmd;
mavlink_msg_command_long_decode(msg, &cmd);
if (cmd.target_system == mavlink_system.sysid && ((cmd.target_component == mavlink_system.compid) || (cmd.target_component == MAV_COMP_ID_ALL)))
{
bool terminate_link = false;
bool reboot = false;
/* result of the command */
uint8_t result = MAV_RESULT_UNSUPPORTED;
/* supported command handling start */
/* request to set different system mode */
if (cmd.command == MAV_CMD_DO_SET_MODE)
{
mavlink_system.mode = cmd.param1;
}
/* request to arm */
// XXX
/* request for an autopilot reboot */
if (cmd.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && cmd.param1 == 1.0f)
{
reboot = true;
result = MAV_RESULT_ACCEPTED;
mavlink_msg_statustext_send(chan,0,"Rebooting autopilot.. ");
}
/* request for a link shutdown */
if (cmd.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && cmd.param1 == 3.0f)
{
terminate_link = true;
result = MAV_RESULT_ACCEPTED;
mavlink_msg_statustext_send(chan,0,"Terminating MAVLink.. ");
}
/* supported command handling stop */
/* send any requested ACKs */
if (cmd.confirmation > 0)
{
mavlink_msg_command_ack_send(chan, cmd.command, result);
}
/* the two termination / reset commands need special handling */
if (terminate_link)
{
printf("MAVLink: Terminating.. \n");
fflush(stdout);
/* sleep 100 ms to allow UART buffer to empty */
led_off(LED_BLUE);
led_on(LED_AMBER);
int i;
for (i = 0; i < 5; i++)
{
led_toggle(LED_AMBER);
usleep(20000);
}
//terminate other threads:
pthread_cancel(heartbeat_thread);
//terminate this thread (receive_thread)
pthread_exit(NULL);
}
if (reboot)
{
printf("MAVLink: Rebooting system.. \n");
fflush(stdout);
/* sleep 100 ms to allow UART buffer to empty */
led_off(LED_BLUE);
led_on(LED_AMBER);
int i;
for (i = 0; i < 5; i++)
{
led_toggle(LED_BLUE);
led_toggle(LED_AMBER);
usleep(20000);
}
//terminate other threads:
pthread_cancel(heartbeat_thread);
// Reset whole system
/* Resetting CPU */
// FIXME Need check for ARM architecture here
#ifndef NVIC_AIRCR
#define NVIC_AIRCR (*((uint32_t*)0xE000ED0C))
#endif
/* Set the SYSRESETREQ bit to force a reset */
NVIC_AIRCR = 0x05fa0004;
/* Spinning until the board is really reset */
while(true);
}
}
}
/* Handle quadrotor motor setpoints */
if(msg->msgid == MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT)
{
mavlink_set_quad_motors_setpoint_t quad_motors_setpoint;
mavlink_msg_set_quad_motors_setpoint_decode(msg, &quad_motors_setpoint);
if(quad_motors_setpoint.target_system == mavlink_system.sysid)
{
global_data_lock(&global_data_quad_motors_setpoint.access_conf);
global_data_quad_motors_setpoint.motor_front_nw = quad_motors_setpoint.motor_front_nw;
global_data_quad_motors_setpoint.motor_right_ne = quad_motors_setpoint.motor_right_ne;
global_data_quad_motors_setpoint.motor_back_se = quad_motors_setpoint.motor_back_se;
global_data_quad_motors_setpoint.motor_left_sw = quad_motors_setpoint.motor_left_sw;
global_data_quad_motors_setpoint.counter++;
global_data_quad_motors_setpoint.timestamp = global_data_get_timestamp_milliseconds();
global_data_unlock(&global_data_quad_motors_setpoint.access_conf);
/* Inform the other processes that there is new gps data available */
global_data_broadcast(&global_data_quad_motors_setpoint.access_conf);
}
}
}
