diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 8252588fac..b8420c46f7 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -878,7 +878,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) uint8_t result = MAV_RESULT_UNSUPPORTED; // do command - send_text_P(MAV_SEVERITY_WARNING,"command received: "); + send_text(MAV_SEVERITY_WARNING,"command received: "); switch(packet.command) { @@ -1001,7 +1001,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } } else { - send_text_P(MAV_SEVERITY_WARNING, "Unsupported preflight calibration"); + send_text(MAV_SEVERITY_WARNING, "Unsupported preflight calibration"); } break; @@ -1137,10 +1137,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { // 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) - 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 handle_param_request_list(msg); break; @@ -1430,7 +1430,7 @@ void Rover::gcs_send_text(MAV_SEVERITY severity, const prog_char_t *str) { for (uint8_t i=0; i= copter.rally.get_rally_total() || 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; } 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; } @@ -1864,7 +1864,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) rally_point.flags = packet.flags; 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; @@ -1872,27 +1872,27 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) //send a rally point to the GCS 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_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()) { - send_text_P(MAV_SEVERITY_WARNING, "bad rally point index"); + send_text(MAV_SEVERITY_WARNING, "bad rally point index"); 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; 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; } - //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, 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.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; } @@ -2034,7 +2034,7 @@ void Copter::gcs_send_text(MAV_SEVERITY severity, const prog_char_t *str) { for (uint8_t i=0; icontrol_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; return 1; } // check we are landed 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; return 1; } @@ -95,13 +95,13 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan) AP_Notify::flags.esc_calibration = true; // 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 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{ - 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 @@ -245,10 +245,10 @@ uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan) } compass.save_motor_compensation(); // 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 { // 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); } diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index b74205621f..35dcb80942 100644 --- a/ArduCopter/motor_test.cpp +++ b/ArduCopter/motor_test.cpp @@ -74,19 +74,19 @@ bool Copter::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc) // check rc has been calibrated pre_arm_rc_checks(); 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; } // ensure we are landed 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; } // check if safety switch has been pushed 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; } diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 53bbc83e32..435df1fedc 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1135,7 +1135,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) uint8_t result = MAV_RESULT_UNSUPPORTED; // do command - send_text_P(MAV_SEVERITY_WARNING,"command received: "); + send_text(MAV_SEVERITY_WARNING,"command received: "); switch(packet.command) { @@ -1267,7 +1267,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } } else { - send_text_P(MAV_SEVERITY_WARNING, "Unsupported preflight calibration"); + send_text(MAV_SEVERITY_WARNING, "Unsupported preflight calibration"); } plane.in_calibration = false; break; @@ -1386,7 +1386,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) plane.auto_state.commanded_go_around = true; 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 { 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: { // 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) - 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 handle_param_request_list(msg); break; @@ -1612,11 +1612,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_fence_point_t packet; mavlink_msg_fence_point_decode(msg, &packet); 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) { - 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) { - 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 { Vector2l point; 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_msg_fence_fetch_point_decode(msg, &packet); 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 { 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, @@ -1648,12 +1648,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (packet.idx >= plane.rally.get_rally_total() || 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; } 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; } @@ -1673,12 +1673,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_rally_fetch_point_t packet; mavlink_msg_rally_fetch_point_decode(msg, &packet); 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; } RallyLocation 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; } @@ -1996,7 +1996,7 @@ void Plane::gcs_send_text(MAV_SEVERITY severity, const prog_char_t *str) { for (uint8_t i=0; i