mirror of https://github.com/ArduPilot/ardupilot
GCS_MAVLink: use GCS_SEND_TEXT rather than gcs().send_text
Co-authored-by: muramura <ma2maru@gmail.com>
This commit is contained in:
parent
c0b905f6e6
commit
4dac24796d
|
@ -125,7 +125,7 @@ void GCS::send_named_float(const char *name, float value) const
|
|||
void GCS::enable_high_latency_connections(bool enabled)
|
||||
{
|
||||
high_latency_link_enabled = enabled;
|
||||
gcs().send_text(MAV_SEVERITY_NOTICE, "High Latency %s", enabled ? "enabled" : "disabled");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "High Latency %s", enabled ? "enabled" : "disabled");
|
||||
}
|
||||
|
||||
bool GCS::get_high_latency_status()
|
||||
|
|
|
@ -1137,7 +1137,7 @@ bool GCS_MAVLINK::set_mavlink_message_id_interval(const uint32_t mavlink_id,
|
|||
{
|
||||
const ap_message id = mavlink_id_to_ap_message_id(mavlink_id);
|
||||
if (id == MSG_LAST) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "No ap_message for mavlink id (%u)", (unsigned int)mavlink_id);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "No ap_message for mavlink id (%u)", (unsigned int)mavlink_id);
|
||||
return false;
|
||||
}
|
||||
return set_ap_message_interval(id, interval_ms);
|
||||
|
@ -1901,7 +1901,7 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us)
|
|||
|
||||
if (uint16_t(now16_ms - try_send_message_stats.statustext_last_sent_ms) > 10000U) {
|
||||
if (try_send_message_stats.longest_time_us) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO,
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
|
||||
"GCS.chan(%u): ap_msg=%u took %uus to send",
|
||||
chan,
|
||||
try_send_message_stats.longest_id,
|
||||
|
@ -1910,42 +1910,42 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us)
|
|||
}
|
||||
if (try_send_message_stats.no_space_for_message &&
|
||||
(is_active() || is_streaming())) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO,
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
|
||||
"GCS.chan(%u): out-of-space: %u",
|
||||
chan,
|
||||
try_send_message_stats.no_space_for_message);
|
||||
try_send_message_stats.no_space_for_message = 0;
|
||||
}
|
||||
if (try_send_message_stats.out_of_time) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO,
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
|
||||
"GCS.chan(%u): out-of-time=%u",
|
||||
chan,
|
||||
try_send_message_stats.out_of_time);
|
||||
try_send_message_stats.out_of_time = 0;
|
||||
}
|
||||
if (max_slowdown_ms) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO,
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
|
||||
"GCS.chan(%u): max slowdown=%u",
|
||||
chan,
|
||||
max_slowdown_ms);
|
||||
max_slowdown_ms = 0;
|
||||
}
|
||||
if (try_send_message_stats.behind) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO,
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
|
||||
"GCS.chan(%u): behind=%u",
|
||||
chan,
|
||||
try_send_message_stats.behind);
|
||||
try_send_message_stats.behind = 0;
|
||||
}
|
||||
if (try_send_message_stats.fnbts_maxtime) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO,
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
|
||||
"GCS.chan(%u): fnbts_maxtime=%uus",
|
||||
chan,
|
||||
try_send_message_stats.fnbts_maxtime);
|
||||
try_send_message_stats.fnbts_maxtime = 0;
|
||||
}
|
||||
if (try_send_message_stats.max_retry_deferred_body_us) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO,
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
|
||||
"GCS.chan(%u): retry_body_maxtime=%uus (%u)",
|
||||
chan,
|
||||
try_send_message_stats.max_retry_deferred_body_us,
|
||||
|
@ -1955,7 +1955,7 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us)
|
|||
}
|
||||
|
||||
for (uint8_t i=0; i<ARRAY_SIZE(deferred_message_bucket); i++) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO,
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
|
||||
"B. intvl. (%u): %u %u %u %u %u",
|
||||
chan,
|
||||
deferred_message_bucket[0].interval_ms,
|
||||
|
@ -3035,7 +3035,7 @@ MAV_RESULT GCS_MAVLINK::set_message_interval(uint32_t msg_id, int32_t interval_u
|
|||
{
|
||||
const ap_message id = mavlink_id_to_ap_message_id(msg_id);
|
||||
if (id == MSG_LAST) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "No ap_message for mavlink id (%u)", (unsigned int)msg_id);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "No ap_message for mavlink id (%u)", (unsigned int)msg_id);
|
||||
return MAV_RESULT_DENIED;
|
||||
}
|
||||
|
||||
|
@ -3523,7 +3523,7 @@ void GCS_MAVLINK::handle_timesync(const mavlink_message_t &msg)
|
|||
return;
|
||||
}
|
||||
#if 0
|
||||
gcs().send_text(MAV_SEVERITY_INFO,
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO,
|
||||
"timesync response sysid=%u (latency=%fms)",
|
||||
msg.sysid,
|
||||
round_trip_time_us*0.001f);
|
||||
|
@ -4545,7 +4545,7 @@ void GCS_MAVLINK::send_sim_state() const
|
|||
MAV_RESULT GCS_MAVLINK::handle_command_flash_bootloader(const mavlink_command_int_t &packet)
|
||||
{
|
||||
if (packet.x != 290876) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Magic not set");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Magic not set");
|
||||
return MAV_RESULT_FAILED;
|
||||
}
|
||||
|
||||
|
@ -4556,7 +4556,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_flash_bootloader(const mavlink_command_in
|
|||
return MAV_RESULT_ACCEPTED;
|
||||
#if AP_SIGNED_FIRMWARE
|
||||
case AP_HAL::Util::FlashBootloader::NOT_SIGNED:
|
||||
gcs().send_text(MAV_SEVERITY_ERROR, "Bootloader not signed");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Bootloader not signed");
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
|
@ -4570,9 +4570,9 @@ MAV_RESULT GCS_MAVLINK::handle_command_flash_bootloader(const mavlink_command_in
|
|||
MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration_baro(const mavlink_message_t &msg)
|
||||
{
|
||||
// fast barometer calibration
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Updating barometer calibration");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Updating barometer calibration");
|
||||
AP::baro().update_calibration();
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Barometer calibration complete");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Barometer calibration complete");
|
||||
|
||||
#if AP_AIRSPEED_ENABLED
|
||||
|
||||
|
@ -4666,7 +4666,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_preflight_calibration(const mavlink_comma
|
|||
{
|
||||
if (hal.util->get_soft_armed()) {
|
||||
// *preflight*, remember?
|
||||
gcs().send_text(MAV_SEVERITY_NOTICE, "Disarm to allow calibration");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Disarm to allow calibration");
|
||||
return MAV_RESULT_FAILED;
|
||||
}
|
||||
// now call subclass methods:
|
||||
|
@ -6401,7 +6401,7 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
|
|||
// message as part of send_message.
|
||||
// This message will be sent out at the same rate as the
|
||||
// unknown message, so should be safe.
|
||||
gcs().send_text(MAV_SEVERITY_DEBUG, "Sending unknown message (%u)", id);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "Sending unknown message (%u)", id);
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
AP_HAL::panic("Sending unknown ap_message %u", id);
|
||||
#endif
|
||||
|
@ -6747,7 +6747,7 @@ void GCS::update_passthru(void)
|
|||
_passthru.baud2 = baud2;
|
||||
_passthru.parity1 = parity1 = _passthru.port1->get_parity();
|
||||
_passthru.parity2 = parity2 = _passthru.port2->get_parity();
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Passthru enabled");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Passthru enabled");
|
||||
if (!_passthru.timer_installed) {
|
||||
_passthru.timer_installed = true;
|
||||
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&GCS::passthru_timer, void));
|
||||
|
@ -6772,7 +6772,7 @@ void GCS::update_passthru(void)
|
|||
if (_passthru.parity2 != parity2) {
|
||||
_passthru.port2->configure_parity(parity2);
|
||||
}
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Passthru disabled");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Passthru disabled");
|
||||
} else if (enabled &&
|
||||
_passthru.timeout_s &&
|
||||
now - _passthru.last_port1_data_ms > uint32_t(_passthru.timeout_s)*1000U) {
|
||||
|
@ -6797,7 +6797,7 @@ void GCS::update_passthru(void)
|
|||
if (_passthru.parity2 != parity2) {
|
||||
_passthru.port2->configure_parity(parity2);
|
||||
}
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Passthru timed out");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Passthru timed out");
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -6893,7 +6893,7 @@ bool GCS_MAVLINK::mavlink_coordinate_frame_to_location_alt_frame(const MAV_FRAME
|
|||
return true;
|
||||
default:
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Unknown mavlink coordinate frame %u", coordinate_frame);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unknown mavlink coordinate frame %u", coordinate_frame);
|
||||
#endif
|
||||
return false;
|
||||
}
|
||||
|
|
|
@ -63,7 +63,7 @@ bool GCS_MAVLINK::ftp_init(void) {
|
|||
failed:
|
||||
delete ftp.requests;
|
||||
ftp.requests = nullptr;
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "failed to initialize MAVFTP");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "failed to initialize MAVFTP");
|
||||
|
||||
return false;
|
||||
}
|
||||
|
@ -572,7 +572,7 @@ void GCS_MAVLINK::ftp_worker(void) {
|
|||
case FTP_OP::TruncateFile:
|
||||
default:
|
||||
// this was bad data, just nack it
|
||||
gcs().send_text(MAV_SEVERITY_DEBUG, "Unsupported FTP: %d", static_cast<int>(request.opcode));
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "Unsupported FTP: %d", static_cast<int>(request.opcode));
|
||||
ftp_error(reply, FTP_ERROR::Fail);
|
||||
break;
|
||||
}
|
||||
|
|
|
@ -290,7 +290,7 @@ void GCS_MAVLINK::handle_param_set(const mavlink_message_t &msg)
|
|||
}
|
||||
|
||||
if ((parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) || vp->is_read_only()) {
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "Param write denied (%s)", key);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Param write denied (%s)", key);
|
||||
// send the readonly value
|
||||
send_parameter_value(key, var_type, old_value);
|
||||
return;
|
||||
|
|
|
@ -101,7 +101,7 @@ bool MAVLink_routing::check_and_forward(GCS_MAVLINK &in_link, const mavlink_mess
|
|||
// of mavlink messages to a private channel (Solo Gimbal case)
|
||||
if (!gopro_status_check && (msg.msgid == MAVLINK_MSG_ID_GOPRO_HEARTBEAT)) {
|
||||
gopro_status_check = true;
|
||||
gcs().send_text(MAV_SEVERITY_NOTICE, "GoPro in Solo gimbal detected");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "GoPro in Solo gimbal detected");
|
||||
}
|
||||
#endif // HAL_SOLO_GIMBAL_ENABLED
|
||||
|
||||
|
|
|
@ -47,7 +47,7 @@ bool MissionItemProtocol::mavlink2_requirement_met(const GCS_MAVLINK &_link, con
|
|||
if (!_link.sending_mavlink1()) {
|
||||
return true;
|
||||
}
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "Need mavlink2 for item transfer");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Need mavlink2 for item transfer");
|
||||
send_mission_ack(_link, msg, MAV_MISSION_UNSUPPORTED);
|
||||
return false;
|
||||
}
|
||||
|
@ -199,7 +199,7 @@ void MissionItemProtocol::handle_mission_request(GCS_MAVLINK &_link,
|
|||
|
||||
if (!mission_request_warning_sent) {
|
||||
mission_request_warning_sent = true;
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "got MISSION_REQUEST; use MISSION_REQUEST_INT!");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "got MISSION_REQUEST; use MISSION_REQUEST_INT!");
|
||||
}
|
||||
|
||||
// buffer space is checked by send_message
|
||||
|
@ -213,7 +213,7 @@ void MissionItemProtocol::send_mission_item_warning()
|
|||
return;
|
||||
}
|
||||
mission_item_warning_sent = true;
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "got MISSION_ITEM; GCS should send MISSION_ITEM_INT");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "got MISSION_ITEM; GCS should send MISSION_ITEM_INT");
|
||||
}
|
||||
|
||||
void MissionItemProtocol::handle_mission_write_partial_list(GCS_MAVLINK &_link,
|
||||
|
@ -238,7 +238,7 @@ void MissionItemProtocol::handle_mission_write_partial_list(GCS_MAVLINK &_link,
|
|||
if ((unsigned)packet.start_index > item_count() ||
|
||||
(unsigned)packet.end_index > item_count() ||
|
||||
packet.end_index < packet.start_index) {
|
||||
gcs().send_text(MAV_SEVERITY_WARNING,"Flight plan update rejected"); // FIXME: Remove this anytime after 2020-01-22
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"Flight plan update rejected"); // FIXME: Remove this anytime after 2020-01-22
|
||||
send_mission_ack(_link, msg, MAV_MISSION_ERROR);
|
||||
return;
|
||||
}
|
||||
|
|
|
@ -222,7 +222,7 @@ MAV_MISSION_RESULT MissionItemProtocol_Fence::allocate_receive_resources(const u
|
|||
if (allocation_size != 0) {
|
||||
_new_items = (AC_PolyFenceItem*)malloc(allocation_size);
|
||||
if (_new_items == nullptr) {
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "Out of memory for upload");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Out of memory for upload");
|
||||
return MAV_MISSION_ERROR;
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue