diff --git a/ArduCopterMega/GCS.h b/ArduCopterMega/GCS.h index 5677e47fbc..907f5bcde7 100644 --- a/ArduCopterMega/GCS.h +++ b/ArduCopterMega/GCS.h @@ -65,6 +65,13 @@ public: /// void send_text(uint8_t severity, const char *str) {} + /// Send a text message with a PSTR() + /// + /// @param severity A value describing the importance of the message. + /// @param str The text to be sent. + /// + void send_text_P(uint8_t severity, const char *str) {} + /// Send acknowledgement for a message. /// /// @param id The message ID being acknowledged. @@ -133,6 +140,7 @@ public: void init(BetterStream *port); void send_message(uint8_t id, uint32_t param = 0); void send_text(uint8_t severity, const char *str); + void send_text_P(uint8_t severity, const char *str); void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2); void data_stream_send(uint16_t freqMin, uint16_t freqMax); private: diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index 41800a6457..0c9ac3ac54 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -53,13 +53,13 @@ GCS_MAVLINK::update(void) // stop waypoint sending if timeout if (global_data.waypoint_sending && (millis() - global_data.waypoint_timelast_send) > global_data.waypoint_send_timeout){ - send_text(SEVERITY_LOW,PSTR("waypoint send timeout")); + send_text_P(SEVERITY_LOW,PSTR("waypoint send timeout")); global_data.waypoint_sending = false; } // stop waypoint receiving if timeout if (global_data.waypoint_receiving && (millis() - global_data.waypoint_timelast_receive) > global_data.waypoint_receive_timeout){ - send_text(SEVERITY_LOW,PSTR("waypoint receive timeout")); + send_text_P(SEVERITY_LOW,PSTR("waypoint receive timeout")); global_data.waypoint_receiving = false; } } @@ -116,6 +116,18 @@ GCS_MAVLINK::send_text(uint8_t severity, const char *str) mavlink_send_text(chan,severity,str); } +void +GCS_MAVLINK::send_text_P(uint8_t severity, const char *str) +{ + mavlink_statustext_t m; + uint8_t i; + for (i=0; icompid, type); - send_text(SEVERITY_LOW,PSTR("flight plan received")); + send_text_P(SEVERITY_LOW,PSTR("flight plan received")); global_data.waypoint_receiving = false; // XXX ignores waypoint radius for individual waypoints, can // only set WP_RADIUS parameter diff --git a/ArduCopterMega/HIL.h b/ArduCopterMega/HIL.h index f312f31d6b..12a31f498c 100644 --- a/ArduCopterMega/HIL.h +++ b/ArduCopterMega/HIL.h @@ -66,6 +66,13 @@ public: /// void send_text(uint8_t severity, const char *str) {} + /// Send a text message with a PSTR() + /// + /// @param severity A value describing the importance of the message. + /// @param str The text to be sent. + /// + void send_text_P(uint8_t severity, const char *str) {} + /// Send acknowledgement for a message. /// /// @param id The message ID being acknowledged. @@ -107,6 +114,7 @@ public: void init(BetterStream *port); void send_message(uint8_t id, uint32_t param = 0); void send_text(uint8_t severity, const char *str); + void send_text_P(uint8_t severity, const char *str); void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2); private: void output_HIL(); diff --git a/ArduCopterMega/HIL_Xplane.pde b/ArduCopterMega/HIL_Xplane.pde index 59e6f4bbe8..f29180a122 100644 --- a/ArduCopterMega/HIL_Xplane.pde +++ b/ArduCopterMega/HIL_Xplane.pde @@ -118,6 +118,11 @@ HIL_XPLANE::send_text(uint8_t severity, const char *str) { } +void +HIL_XPLANE::send_text_P(uint8_t severity, const char *str) +{ +} + void HIL_XPLANE::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) { diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index 5a0d5004fd..14f974ac66 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -253,7 +253,7 @@ void start_new_log(byte num_existing_logs) DataFlash.FinishWrite(); DataFlash.StartWrite(start_pages[num_existing_logs-1]); }else{ - gcs.send_text(SEVERITY_LOW," Logs full - logging discontinued"); + gcs.send_text_P(SEVERITY_LOW,PSTR(" Logs full - logging discontinued")); } } diff --git a/ArduCopterMega/Mavlink_Common.h b/ArduCopterMega/Mavlink_Common.h index 08f78912e3..77818a4d7f 100644 --- a/ArduCopterMega/Mavlink_Common.h +++ b/ArduCopterMega/Mavlink_Common.h @@ -16,7 +16,7 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) return 1; }else if(compid != mavlink_system.compid){ - gcs.send_text(SEVERITY_LOW,"component id mismatch"); + gcs.send_text_P(SEVERITY_LOW,PSTR("component id mismatch")); return 0; // XXX currently not receiving correct compid from gcs }else{ diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index 7365939e43..1d34a2860e 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -133,7 +133,7 @@ It looks to see what the next command type is and finds the last command. */ void set_next_WP(struct Location *wp) { - //GCS.send_text(SEVERITY_LOW,"load WP"); + //GCS.send_text_P(SEVERITY_LOW,PSTR("load WP")); SendDebug("MSG wp_index: "); SendDebugln(g.waypoint_index, DEC); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde index 1a01eb859c..a0a1a2b4eb 100644 --- a/ArduCopterMega/commands_logic.pde +++ b/ArduCopterMega/commands_logic.pde @@ -153,7 +153,7 @@ bool verify_must() break; default: - //gcs.send_text(SEVERITY_HIGH," No current Must commands"); + //gcs.send_text_P(SEVERITY_HIGH,PSTR(" No current Must commands")); return false; break; } @@ -180,7 +180,7 @@ bool verify_may() break; default: - //gcs.send_text(SEVERITY_HIGH," No current May commands"); + //gcs.send_text_P(SEVERITY_HIGH,PSTR(" No current May commands")); return false; break; } @@ -297,7 +297,7 @@ bool verify_nav_wp() // Have we passed the WP? if(loiter_sum > 90){ - gcs.send_text(SEVERITY_MEDIUM,"Missed WP"); + gcs.send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP")); return true; } return false; @@ -311,7 +311,7 @@ bool verify_loiter_unlim() bool verify_loiter_time() { if ((millis() - loiter_time) > (long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds - gcs.send_text(SEVERITY_LOW," LOITER time complete "); + gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete")); return true; } return false; @@ -320,7 +320,7 @@ bool verify_loiter_time() bool verify_RTL() { if (wp_distance <= g.waypoint_radius) { - gcs.send_text(SEVERITY_LOW,"Reached home"); + gcs.send_text_P(SEVERITY_LOW,PSTR("Reached home")); return true; }else{ return false; @@ -552,4 +552,4 @@ void do_repeat_relay() event_delay = next_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) event_repeat = next_command.alt * 2; update_events(); -} \ No newline at end of file +} diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde index 064b1db037..0680c34722 100644 --- a/ArduCopterMega/commands_process.pde +++ b/ArduCopterMega/commands_process.pde @@ -42,7 +42,7 @@ void load_next_command_from_EEPROM() // -------------------------------------------- if(next_command.id == NO_COMMAND){ // we are out of commands! - gcs.send_text(SEVERITY_LOW,"out of commands!"); + gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!")); handle_no_commands(); } } @@ -100,7 +100,7 @@ void process_next_command() /**************************************************/ void process_must() { - gcs.send_text(SEVERITY_LOW,"New cmd: "); + gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: ")); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); // clear May indexes @@ -117,7 +117,7 @@ void process_must() void process_may() { - gcs.send_text(SEVERITY_LOW,""); + gcs.send_text_P(SEVERITY_LOW,PSTR("")); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); command_may_ID = next_command.id; @@ -136,7 +136,7 @@ void process_now() // ----------------------------------------- next_command.id = NO_COMMAND; - gcs.send_text(SEVERITY_LOW, ""); + gcs.send_text_P(SEVERITY_LOW, PSTR("")); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); } diff --git a/ArduCopterMega/events.pde b/ArduCopterMega/events.pde index 504ad380d1..6dfd922554 100644 --- a/ArduCopterMega/events.pde +++ b/ArduCopterMega/events.pde @@ -42,7 +42,7 @@ void failsafe_off_event() void low_battery_event(void) { - gcs.send_text(SEVERITY_HIGH,"Low Battery!"); + gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); set_mode(RTL); g.throttle_cruise.set(THROTTLE_CRUISE); } diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index fdb571957c..7e47c306c2 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -21,7 +21,7 @@ void navigate() wp_distance = get_distance(¤t_loc, &next_WP); if (wp_distance < 0){ - gcs.send_text(SEVERITY_HIGH," WP error - distance < 0"); + gcs.send_text_P(SEVERITY_HIGH,PSTR(" WP error - distance < 0")); //Serial.println(wp_distance,DEC); //print_current_waypoints(); return; diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 56350e7722..d90ae17f53 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -215,10 +215,10 @@ void init_ardupilot() //******************************************************************************** void startup_ground(void) { - gcs.send_text(SEVERITY_LOW," GROUND START"); + gcs.send_text_P(SEVERITY_LOW,PSTR(" GROUND START")); #if(GROUND_START_DELAY > 0) - //gcs.send_text(SEVERITY_LOW," With Delay"); + //gcs.send_text_P(SEVERITY_LOW, PSTR(" With Delay")); delay(GROUND_START_DELAY * 1000); #endif @@ -255,7 +255,7 @@ void startup_ground(void) // ------------------- init_commands(); - gcs.send_text(SEVERITY_LOW,"\n\n Ready to FLY."); + gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY.")); } void set_mode(byte mode) @@ -274,7 +274,7 @@ void set_mode(byte mode) // disarm motors temp motor_auto_safe = false; } - //send_text(SEVERITY_LOW,"control mode"); + //send_text_P(SEVERITY_LOW,PSTR("control mode")); //Serial.printf("set mode: %d old: %d\n", (int)mode, (int)control_mode); switch(control_mode) {