/** * @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); } } }