mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-03 04:03:59 -04:00
Replace use of send_text_P() with send_text()
This commit is contained in:
parent
831d8acca5
commit
89fc4f4b62
@ -878,7 +878,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
uint8_t result = MAV_RESULT_UNSUPPORTED;
|
uint8_t result = MAV_RESULT_UNSUPPORTED;
|
||||||
|
|
||||||
// do command
|
// do command
|
||||||
send_text_P(MAV_SEVERITY_WARNING,"command received: ");
|
send_text(MAV_SEVERITY_WARNING,"command received: ");
|
||||||
|
|
||||||
switch(packet.command) {
|
switch(packet.command) {
|
||||||
|
|
||||||
@ -1001,7 +1001,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
send_text_P(MAV_SEVERITY_WARNING, "Unsupported preflight calibration");
|
send_text(MAV_SEVERITY_WARNING, "Unsupported preflight calibration");
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
@ -1137,10 +1137,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
|
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
|
||||||
{
|
{
|
||||||
// mark the firmware version in the tlog
|
// mark the firmware version in the tlog
|
||||||
send_text_P(MAV_SEVERITY_WARNING, FIRMWARE_STRING);
|
send_text(MAV_SEVERITY_WARNING, FIRMWARE_STRING);
|
||||||
|
|
||||||
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
|
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
|
||||||
send_text_P(MAV_SEVERITY_WARNING, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);
|
send_text(MAV_SEVERITY_WARNING, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);
|
||||||
#endif
|
#endif
|
||||||
handle_param_request_list(msg);
|
handle_param_request_list(msg);
|
||||||
break;
|
break;
|
||||||
@ -1430,7 +1430,7 @@ void Rover::gcs_send_text(MAV_SEVERITY severity, const prog_char_t *str)
|
|||||||
{
|
{
|
||||||
for (uint8_t i=0; i<num_gcs; i++) {
|
for (uint8_t i=0; i<num_gcs; i++) {
|
||||||
if (gcs[i].initialised) {
|
if (gcs[i].initialised) {
|
||||||
gcs[i].send_text_P(severity, str);
|
gcs[i].send_text(severity, str);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#if LOGGING_ENABLED == ENABLED
|
#if LOGGING_ENABLED == ENABLED
|
||||||
|
@ -599,7 +599,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
uint8_t result = MAV_RESULT_UNSUPPORTED;
|
uint8_t result = MAV_RESULT_UNSUPPORTED;
|
||||||
|
|
||||||
// do command
|
// do command
|
||||||
send_text_P(MAV_SEVERITY_WARNING,"command received: ");
|
send_text(MAV_SEVERITY_WARNING,"command received: ");
|
||||||
|
|
||||||
switch(packet.command) {
|
switch(packet.command) {
|
||||||
|
|
||||||
@ -827,7 +827,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
// check if this is the HOME wp
|
// check if this is the HOME wp
|
||||||
if (packet.seq == 0) {
|
if (packet.seq == 0) {
|
||||||
tracker.set_home(tell_command); // New home in EEPROM
|
tracker.set_home(tell_command); // New home in EEPROM
|
||||||
send_text_P(MAV_SEVERITY_WARNING,"new HOME received");
|
send_text(MAV_SEVERITY_WARNING,"new HOME received");
|
||||||
waypoint_receiving = false;
|
waypoint_receiving = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -964,7 +964,7 @@ void Tracker::gcs_send_text(MAV_SEVERITY severity, const prog_char_t *str)
|
|||||||
{
|
{
|
||||||
for (uint8_t i=0; i<num_gcs; i++) {
|
for (uint8_t i=0; i<num_gcs; i++) {
|
||||||
if (gcs[i].initialised) {
|
if (gcs[i].initialised) {
|
||||||
gcs[i].send_text_P(severity, str);
|
gcs[i].send_text(severity, str);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#if LOGGING_ENABLED == ENABLED
|
#if LOGGING_ENABLED == ENABLED
|
||||||
|
@ -1022,12 +1022,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: // MAV ID: 21
|
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: // MAV ID: 21
|
||||||
{
|
{
|
||||||
// mark the firmware version in the tlog
|
// mark the firmware version in the tlog
|
||||||
send_text_P(MAV_SEVERITY_WARNING, FIRMWARE_STRING);
|
send_text(MAV_SEVERITY_WARNING, FIRMWARE_STRING);
|
||||||
|
|
||||||
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
|
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
|
||||||
send_text_P(MAV_SEVERITY_WARNING, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);
|
send_text(MAV_SEVERITY_WARNING, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);
|
||||||
#endif
|
#endif
|
||||||
send_text_P(MAV_SEVERITY_WARNING, "Frame: " FRAME_CONFIG_STRING);
|
send_text(MAV_SEVERITY_WARNING, "Frame: " FRAME_CONFIG_STRING);
|
||||||
handle_param_request_list(msg);
|
handle_param_request_list(msg);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@ -1518,13 +1518,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
case MAV_CMD_DO_SEND_BANNER: {
|
case MAV_CMD_DO_SEND_BANNER: {
|
||||||
result = MAV_RESULT_ACCEPTED;
|
result = MAV_RESULT_ACCEPTED;
|
||||||
|
|
||||||
send_text_P(MAV_SEVERITY_WARNING, FIRMWARE_STRING);
|
send_text(MAV_SEVERITY_WARNING, FIRMWARE_STRING);
|
||||||
|
|
||||||
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
|
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
|
||||||
send_text_P(MAV_SEVERITY_WARNING, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);
|
send_text(MAV_SEVERITY_WARNING, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
send_text_P(MAV_SEVERITY_WARNING, "Frame: " FRAME_CONFIG_STRING);
|
send_text(MAV_SEVERITY_WARNING, "Frame: " FRAME_CONFIG_STRING);
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD != HAL_BOARD_APM1 && CONFIG_HAL_BOARD != HAL_BOARD_APM2
|
#if CONFIG_HAL_BOARD != HAL_BOARD_APM1 && CONFIG_HAL_BOARD != HAL_BOARD_APM2
|
||||||
// send system ID if we can
|
// send system ID if we can
|
||||||
@ -1846,12 +1846,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
|
|
||||||
if (packet.idx >= copter.rally.get_rally_total() ||
|
if (packet.idx >= copter.rally.get_rally_total() ||
|
||||||
packet.idx >= copter.rally.get_rally_max()) {
|
packet.idx >= copter.rally.get_rally_max()) {
|
||||||
send_text_P(MAV_SEVERITY_WARNING,"bad rally point message ID");
|
send_text(MAV_SEVERITY_WARNING,"bad rally point message ID");
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (packet.count != copter.rally.get_rally_total()) {
|
if (packet.count != copter.rally.get_rally_total()) {
|
||||||
send_text_P(MAV_SEVERITY_WARNING,"bad rally point message count");
|
send_text(MAV_SEVERITY_WARNING,"bad rally point message count");
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -1864,7 +1864,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
rally_point.flags = packet.flags;
|
rally_point.flags = packet.flags;
|
||||||
|
|
||||||
if (!copter.rally.set_rally_point_with_index(packet.idx, rally_point)) {
|
if (!copter.rally.set_rally_point_with_index(packet.idx, rally_point)) {
|
||||||
send_text_P(MAV_SEVERITY_CRITICAL, "error setting rally point");
|
send_text(MAV_SEVERITY_CRITICAL, "error setting rally point");
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
@ -1872,27 +1872,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
|
|
||||||
//send a rally point to the GCS
|
//send a rally point to the GCS
|
||||||
case MAVLINK_MSG_ID_RALLY_FETCH_POINT: {
|
case MAVLINK_MSG_ID_RALLY_FETCH_POINT: {
|
||||||
//send_text_P(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 1"); // #### TEMP
|
//send_text(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 1"); // #### TEMP
|
||||||
|
|
||||||
mavlink_rally_fetch_point_t packet;
|
mavlink_rally_fetch_point_t packet;
|
||||||
mavlink_msg_rally_fetch_point_decode(msg, &packet);
|
mavlink_msg_rally_fetch_point_decode(msg, &packet);
|
||||||
|
|
||||||
//send_text_P(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 2"); // #### TEMP
|
//send_text(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 2"); // #### TEMP
|
||||||
|
|
||||||
if (packet.idx > copter.rally.get_rally_total()) {
|
if (packet.idx > copter.rally.get_rally_total()) {
|
||||||
send_text_P(MAV_SEVERITY_WARNING, "bad rally point index");
|
send_text(MAV_SEVERITY_WARNING, "bad rally point index");
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
//send_text_P(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 3"); // #### TEMP
|
//send_text(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 3"); // #### TEMP
|
||||||
|
|
||||||
RallyLocation rally_point;
|
RallyLocation rally_point;
|
||||||
if (!copter.rally.get_rally_point_with_index(packet.idx, rally_point)) {
|
if (!copter.rally.get_rally_point_with_index(packet.idx, rally_point)) {
|
||||||
send_text_P(MAV_SEVERITY_WARNING, "failed to set rally point");
|
send_text(MAV_SEVERITY_WARNING, "failed to set rally point");
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
//send_text_P(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 4"); // #### TEMP
|
//send_text(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 4"); // #### TEMP
|
||||||
|
|
||||||
mavlink_msg_rally_point_send_buf(msg,
|
mavlink_msg_rally_point_send_buf(msg,
|
||||||
chan, msg->sysid, msg->compid, packet.idx,
|
chan, msg->sysid, msg->compid, packet.idx,
|
||||||
@ -1900,7 +1900,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
rally_point.alt, rally_point.break_alt, rally_point.land_dir,
|
rally_point.alt, rally_point.break_alt, rally_point.land_dir,
|
||||||
rally_point.flags);
|
rally_point.flags);
|
||||||
|
|
||||||
//send_text_P(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 5"); // #### TEMP
|
//send_text(MAV_SEVERITY_CRITICAL, "## getting rally point in GCS_Mavlink.cpp 5"); // #### TEMP
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@ -2034,7 +2034,7 @@ void Copter::gcs_send_text(MAV_SEVERITY severity, const prog_char_t *str)
|
|||||||
{
|
{
|
||||||
for (uint8_t i=0; i<num_gcs; i++) {
|
for (uint8_t i=0; i<num_gcs; i++) {
|
||||||
if (gcs[i].initialised) {
|
if (gcs[i].initialised) {
|
||||||
gcs[i].send_text_P(severity, str);
|
gcs[i].send_text(severity, str);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -37,7 +37,7 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan)
|
|||||||
|
|
||||||
// check compass is enabled
|
// check compass is enabled
|
||||||
if (!g.compass_enabled) {
|
if (!g.compass_enabled) {
|
||||||
gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, "compass disabled\n");
|
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "compass disabled\n");
|
||||||
ap.compass_mot = false;
|
ap.compass_mot = false;
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
@ -46,7 +46,7 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan)
|
|||||||
compass.read();
|
compass.read();
|
||||||
for (uint8_t i=0; i<compass.get_count(); i++) {
|
for (uint8_t i=0; i<compass.get_count(); i++) {
|
||||||
if (!compass.healthy(i)) {
|
if (!compass.healthy(i)) {
|
||||||
gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, "check compass");
|
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "check compass");
|
||||||
ap.compass_mot = false;
|
ap.compass_mot = false;
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
@ -55,7 +55,7 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan)
|
|||||||
// check if radio is calibrated
|
// check if radio is calibrated
|
||||||
pre_arm_rc_checks();
|
pre_arm_rc_checks();
|
||||||
if (!ap.pre_arm_rc_check) {
|
if (!ap.pre_arm_rc_check) {
|
||||||
gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, "RC not calibrated");
|
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "RC not calibrated");
|
||||||
ap.compass_mot = false;
|
ap.compass_mot = false;
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
@ -63,14 +63,14 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan)
|
|||||||
// check throttle is at zero
|
// check throttle is at zero
|
||||||
read_radio();
|
read_radio();
|
||||||
if (channel_throttle->control_in != 0) {
|
if (channel_throttle->control_in != 0) {
|
||||||
gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, "thr not zero");
|
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "thr not zero");
|
||||||
ap.compass_mot = false;
|
ap.compass_mot = false;
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
// check we are landed
|
// check we are landed
|
||||||
if (!ap.land_complete) {
|
if (!ap.land_complete) {
|
||||||
gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, "Not landed");
|
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Not landed");
|
||||||
ap.compass_mot = false;
|
ap.compass_mot = false;
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
@ -95,13 +95,13 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan)
|
|||||||
AP_Notify::flags.esc_calibration = true;
|
AP_Notify::flags.esc_calibration = true;
|
||||||
|
|
||||||
// warn user we are starting calibration
|
// warn user we are starting calibration
|
||||||
gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, "STARTING CALIBRATION");
|
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "STARTING CALIBRATION");
|
||||||
|
|
||||||
// inform what type of compensation we are attempting
|
// inform what type of compensation we are attempting
|
||||||
if (comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
|
if (comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
|
||||||
gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, "CURRENT");
|
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "CURRENT");
|
||||||
} else{
|
} else{
|
||||||
gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, "THROTTLE");
|
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "THROTTLE");
|
||||||
}
|
}
|
||||||
|
|
||||||
// disable throttle and battery failsafe
|
// disable throttle and battery failsafe
|
||||||
@ -245,10 +245,10 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan)
|
|||||||
}
|
}
|
||||||
compass.save_motor_compensation();
|
compass.save_motor_compensation();
|
||||||
// display success message
|
// display success message
|
||||||
gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, "Calibration Successful!");
|
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Calibration Successful!");
|
||||||
} else {
|
} else {
|
||||||
// compensation vector never updated, report failure
|
// compensation vector never updated, report failure
|
||||||
gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL, "Failed!");
|
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Failed!");
|
||||||
compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED);
|
compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -74,19 +74,19 @@ bool Copter::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc)
|
|||||||
// check rc has been calibrated
|
// check rc has been calibrated
|
||||||
pre_arm_rc_checks();
|
pre_arm_rc_checks();
|
||||||
if(check_rc && !ap.pre_arm_rc_check) {
|
if(check_rc && !ap.pre_arm_rc_check) {
|
||||||
gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL,"Motor Test: RC not calibrated");
|
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: RC not calibrated");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// ensure we are landed
|
// ensure we are landed
|
||||||
if (!ap.land_complete) {
|
if (!ap.land_complete) {
|
||||||
gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL,"Motor Test: vehicle not landed");
|
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: vehicle not landed");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// check if safety switch has been pushed
|
// check if safety switch has been pushed
|
||||||
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
|
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
|
||||||
gcs[chan-MAVLINK_COMM_0].send_text_P(MAV_SEVERITY_CRITICAL,"Motor Test: Safety Switch");
|
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: Safety Switch");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1135,7 +1135,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
uint8_t result = MAV_RESULT_UNSUPPORTED;
|
uint8_t result = MAV_RESULT_UNSUPPORTED;
|
||||||
|
|
||||||
// do command
|
// do command
|
||||||
send_text_P(MAV_SEVERITY_WARNING,"command received: ");
|
send_text(MAV_SEVERITY_WARNING,"command received: ");
|
||||||
|
|
||||||
switch(packet.command) {
|
switch(packet.command) {
|
||||||
|
|
||||||
@ -1267,7 +1267,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
send_text_P(MAV_SEVERITY_WARNING, "Unsupported preflight calibration");
|
send_text(MAV_SEVERITY_WARNING, "Unsupported preflight calibration");
|
||||||
}
|
}
|
||||||
plane.in_calibration = false;
|
plane.in_calibration = false;
|
||||||
break;
|
break;
|
||||||
@ -1386,7 +1386,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
plane.auto_state.commanded_go_around = true;
|
plane.auto_state.commanded_go_around = true;
|
||||||
|
|
||||||
result = MAV_RESULT_ACCEPTED;
|
result = MAV_RESULT_ACCEPTED;
|
||||||
plane.gcs_send_text(MAV_SEVERITY_CRITICAL,"Go around command accepted.");
|
plane.gcs_send_text(MAV_SEVERITY_CRITICAL,"Go around command accepted.");
|
||||||
} else {
|
} else {
|
||||||
plane.gcs_send_text(MAV_SEVERITY_CRITICAL,"Rejected go around command.");
|
plane.gcs_send_text(MAV_SEVERITY_CRITICAL,"Rejected go around command.");
|
||||||
}
|
}
|
||||||
@ -1550,10 +1550,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
|
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
|
||||||
{
|
{
|
||||||
// mark the firmware version in the tlog
|
// mark the firmware version in the tlog
|
||||||
send_text_P(MAV_SEVERITY_WARNING, FIRMWARE_STRING);
|
send_text(MAV_SEVERITY_WARNING, FIRMWARE_STRING);
|
||||||
|
|
||||||
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
|
#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
|
||||||
send_text_P(MAV_SEVERITY_WARNING, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);
|
send_text(MAV_SEVERITY_WARNING, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION);
|
||||||
#endif
|
#endif
|
||||||
handle_param_request_list(msg);
|
handle_param_request_list(msg);
|
||||||
break;
|
break;
|
||||||
@ -1612,11 +1612,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
mavlink_fence_point_t packet;
|
mavlink_fence_point_t packet;
|
||||||
mavlink_msg_fence_point_decode(msg, &packet);
|
mavlink_msg_fence_point_decode(msg, &packet);
|
||||||
if (plane.g.fence_action != FENCE_ACTION_NONE) {
|
if (plane.g.fence_action != FENCE_ACTION_NONE) {
|
||||||
send_text_P(MAV_SEVERITY_WARNING,"fencing must be disabled");
|
send_text(MAV_SEVERITY_WARNING,"fencing must be disabled");
|
||||||
} else if (packet.count != plane.g.fence_total) {
|
} else if (packet.count != plane.g.fence_total) {
|
||||||
send_text_P(MAV_SEVERITY_WARNING,"bad fence point");
|
send_text(MAV_SEVERITY_WARNING,"bad fence point");
|
||||||
} else if (fabsf(packet.lat) > 90.0f || fabsf(packet.lng) > 180.0f) {
|
} else if (fabsf(packet.lat) > 90.0f || fabsf(packet.lng) > 180.0f) {
|
||||||
send_text_P(MAV_SEVERITY_WARNING,"invalid fence point, lat or lng too large");
|
send_text(MAV_SEVERITY_WARNING,"invalid fence point, lat or lng too large");
|
||||||
} else {
|
} else {
|
||||||
Vector2l point;
|
Vector2l point;
|
||||||
point.x = packet.lat*1.0e7f;
|
point.x = packet.lat*1.0e7f;
|
||||||
@ -1631,7 +1631,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
mavlink_fence_fetch_point_t packet;
|
mavlink_fence_fetch_point_t packet;
|
||||||
mavlink_msg_fence_fetch_point_decode(msg, &packet);
|
mavlink_msg_fence_fetch_point_decode(msg, &packet);
|
||||||
if (packet.idx >= plane.g.fence_total) {
|
if (packet.idx >= plane.g.fence_total) {
|
||||||
send_text_P(MAV_SEVERITY_WARNING,"bad fence point");
|
send_text(MAV_SEVERITY_WARNING,"bad fence point");
|
||||||
} else {
|
} else {
|
||||||
Vector2l point = plane.get_fence_point_with_index(packet.idx);
|
Vector2l point = plane.get_fence_point_with_index(packet.idx);
|
||||||
mavlink_msg_fence_point_send_buf(msg, chan, msg->sysid, msg->compid, packet.idx, plane.g.fence_total,
|
mavlink_msg_fence_point_send_buf(msg, chan, msg->sysid, msg->compid, packet.idx, plane.g.fence_total,
|
||||||
@ -1648,12 +1648,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
|
|
||||||
if (packet.idx >= plane.rally.get_rally_total() ||
|
if (packet.idx >= plane.rally.get_rally_total() ||
|
||||||
packet.idx >= plane.rally.get_rally_max()) {
|
packet.idx >= plane.rally.get_rally_max()) {
|
||||||
send_text_P(MAV_SEVERITY_WARNING,"bad rally point message ID");
|
send_text(MAV_SEVERITY_WARNING,"bad rally point message ID");
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (packet.count != plane.rally.get_rally_total()) {
|
if (packet.count != plane.rally.get_rally_total()) {
|
||||||
send_text_P(MAV_SEVERITY_WARNING,"bad rally point message count");
|
send_text(MAV_SEVERITY_WARNING,"bad rally point message count");
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -1673,12 +1673,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||||||
mavlink_rally_fetch_point_t packet;
|
mavlink_rally_fetch_point_t packet;
|
||||||
mavlink_msg_rally_fetch_point_decode(msg, &packet);
|
mavlink_msg_rally_fetch_point_decode(msg, &packet);
|
||||||
if (packet.idx > plane.rally.get_rally_total()) {
|
if (packet.idx > plane.rally.get_rally_total()) {
|
||||||
send_text_P(MAV_SEVERITY_WARNING, "bad rally point index");
|
send_text(MAV_SEVERITY_WARNING, "bad rally point index");
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
RallyLocation rally_point;
|
RallyLocation rally_point;
|
||||||
if (!plane.rally.get_rally_point_with_index(packet.idx, rally_point)) {
|
if (!plane.rally.get_rally_point_with_index(packet.idx, rally_point)) {
|
||||||
send_text_P(MAV_SEVERITY_WARNING, "failed to set rally point");
|
send_text(MAV_SEVERITY_WARNING, "failed to set rally point");
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -1996,7 +1996,7 @@ void Plane::gcs_send_text(MAV_SEVERITY severity, const prog_char_t *str)
|
|||||||
{
|
{
|
||||||
for (uint8_t i=0; i<num_gcs; i++) {
|
for (uint8_t i=0; i<num_gcs; i++) {
|
||||||
if (gcs[i].initialised) {
|
if (gcs[i].initialised) {
|
||||||
gcs[i].send_text_P(severity, str);
|
gcs[i].send_text(severity, str);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#if LOGGING_ENABLED == ENABLED
|
#if LOGGING_ENABLED == ENABLED
|
||||||
|
Loading…
Reference in New Issue
Block a user