From a64689600f493879e73205d6016a7f587b4eefa2 Mon Sep 17 00:00:00 2001 From: lvale Date: Tue, 3 Nov 2015 05:19:03 +0000 Subject: [PATCH] Rover: text message severity uniformization Global revision of message severity values. Required also change to the low priority function gcs_send_text_fmt() on GCS_Mavlink.cpp to disable the automatic setting of priority on messages sent by this function --- APMrover2/GCS_Mavlink.cpp | 12 ++++++------ APMrover2/Log.cpp | 4 ++-- APMrover2/Rover.h | 2 +- APMrover2/Steering.cpp | 2 +- APMrover2/commands.cpp | 4 ++-- APMrover2/commands_logic.cpp | 18 +++++++++--------- APMrover2/navigation.cpp | 5 ----- APMrover2/sensors.cpp | 12 ++++++------ APMrover2/system.cpp | 16 ++++++++-------- 9 files changed, 35 insertions(+), 40 deletions(-) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 9f0d6d305d..045f35f571 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(MAV_SEVERITY_WARNING,"command received: "); + send_text(MAV_SEVERITY_INFO,"command received: "); switch(packet.command) { @@ -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(MAV_SEVERITY_WARNING, FIRMWARE_STRING); + send_text(MAV_SEVERITY_INFO, FIRMWARE_STRING); #if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION) - send_text(MAV_SEVERITY_WARNING, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION); + send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION); #endif handle_param_request_list(msg); break; @@ -1363,7 +1363,7 @@ void Rover::mavlink_delay_cb() } if (tnow - last_5s > 5000) { last_5s = tnow; - gcs_send_text(MAV_SEVERITY_WARNING, "Initialising APM..."); + gcs_send_text(MAV_SEVERITY_INFO, "Initialising APM..."); } check_usb_mux(); @@ -1440,10 +1440,10 @@ void Rover::gcs_send_text(MAV_SEVERITY severity, const char *str) * only one fits in the queue, so if you send more than one before the * last one gets into the serial buffer then the old one will be lost */ -void Rover::gcs_send_text_fmt(const char *fmt, ...) +void Rover::gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...) { va_list arg_list; - gcs[0].pending_status.severity = (uint8_t)MAV_SEVERITY_WARNING; + gcs[0].pending_status.severity = (uint8_t)severity; va_start(arg_list, fmt); hal.util->vsnprintf((char *)gcs[0].pending_status.text, sizeof(gcs[0].pending_status.text), fmt, arg_list); diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index 2215c45915..e52766b188 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -391,9 +391,9 @@ void Rover::log_init(void) gcs_send_text(MAV_SEVERITY_WARNING, "No dataflash card inserted"); g.log_bitmask.set(0); } else if (DataFlash.NeedPrep()) { - gcs_send_text(MAV_SEVERITY_WARNING, "Preparing log system"); + gcs_send_text(MAV_SEVERITY_INFO, "Preparing log system"); DataFlash.Prep(); - gcs_send_text(MAV_SEVERITY_WARNING, "Prepared log system"); + gcs_send_text(MAV_SEVERITY_INFO, "Prepared log system"); for (uint8_t i=0; i= g.auto_kickstart) { - gcs_send_text_fmt("Triggered AUTO xaccel=%.1f", (double)xaccel); + gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Triggered AUTO xaccel=%.1f", (double)xaccel); auto_triggered = true; return true; } diff --git a/APMrover2/commands.cpp b/APMrover2/commands.cpp index 2874ec8610..f912ca14a8 100644 --- a/APMrover2/commands.cpp +++ b/APMrover2/commands.cpp @@ -30,7 +30,7 @@ void Rover::set_next_WP(const struct Location& loc) // location as the previous waypoint, to prevent immediately // considering the waypoint complete if (location_passed_point(current_loc, prev_WP, next_WP)) { - gcs_send_text(MAV_SEVERITY_WARNING, "Resetting prev_WP"); + gcs_send_text(MAV_SEVERITY_NOTICE, "Resetting prev_WP"); prev_WP = current_loc; } @@ -63,7 +63,7 @@ void Rover::init_home() return; } - gcs_send_text(MAV_SEVERITY_WARNING, "init home"); + gcs_send_text(MAV_SEVERITY_INFO, "init home"); ahrs.set_home(gps.location()); home_is_set = true; diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index 916f110f48..02eb9f44f2 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -17,7 +17,7 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd) return false; } - gcs_send_text_fmt("Executing command ID #%i",cmd.id); + gcs_send_text_fmt(MAV_SEVERITY_INFO, "Executing command ID #%i",cmd.id); // remember the course of our next navigation leg next_navigation_leg_cd = mission.get_next_ground_course_cd(0); @@ -117,7 +117,7 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd) void Rover::exit_mission() { if (control_mode == AUTO) { - gcs_send_text_fmt("No commands. Can't set AUTO - setting HOLD"); + gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "No commands. Can't set AUTO - setting HOLD"); set_mode(HOLD); } } @@ -203,7 +203,7 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd) // Check if we need to loiter at this waypoint if (loiter_time_max > 0) { if (loiter_time == 0) { // check if we are just starting loiter - gcs_send_text_fmt("Reached Waypoint #%i - Loiter for %i seconds", + gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached Waypoint #%i - Loiter for %i seconds", (unsigned)cmd.index, (unsigned)loiter_time_max); // record the current time i.e. start timer @@ -214,7 +214,7 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd) return false; } } else { - gcs_send_text_fmt("Reached Waypoint #%i dist %um", + gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached Waypoint #%i dist %um", (unsigned)cmd.index, (unsigned)get_distance(current_loc, next_WP)); } @@ -228,7 +228,7 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd) // check if we have gone futher past the wp then last time and output new message if we have if ((uint32_t)distance_past_wp != (uint32_t)get_distance(current_loc, next_WP)) { distance_past_wp = get_distance(current_loc, next_WP); - gcs_send_text_fmt("Passed Waypoint #%i dist %um", + gcs_send_text_fmt(MAV_SEVERITY_INFO, "Passed Waypoint #%i dist %um", (unsigned)cmd.index, (unsigned)distance_past_wp); } @@ -248,14 +248,14 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd) bool Rover::verify_RTL() { if (wp_distance <= g.waypoint_radius) { - gcs_send_text(MAV_SEVERITY_WARNING,"Reached Destination"); + gcs_send_text(MAV_SEVERITY_INFO,"Reached Destination"); rtl_complete = true; return true; } // have we gone past the waypoint? if (location_passed_point(current_loc, prev_WP, next_WP)) { - gcs_send_text_fmt("Reached Destination: Distance away %um", + gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached Destination: Distance away %um", (unsigned)get_distance(current_loc, next_WP)); rtl_complete = true; return true; @@ -309,12 +309,12 @@ void Rover::do_change_speed(const AP_Mission::Mission_Command& cmd) { if (cmd.content.speed.target_ms > 0) { g.speed_cruise.set(cmd.content.speed.target_ms); - gcs_send_text_fmt("Cruise speed: %.1f m/s", (double)g.speed_cruise.get()); + gcs_send_text_fmt(MAV_SEVERITY_INFO, "Cruise speed: %.1f m/s", (double)g.speed_cruise.get()); } if (cmd.content.speed.throttle_pct > 0 && cmd.content.speed.throttle_pct <= 100) { g.throttle_cruise.set(cmd.content.speed.throttle_pct); - gcs_send_text_fmt("Cruise throttle: %.1f", g.throttle_cruise.get()); + gcs_send_text_fmt(MAV_SEVERITY_INFO, "Cruise throttle: %.1f", g.throttle_cruise.get()); } } diff --git a/APMrover2/navigation.cpp b/APMrover2/navigation.cpp index be7ce3d3d9..c409336fdc 100644 --- a/APMrover2/navigation.cpp +++ b/APMrover2/navigation.cpp @@ -21,11 +21,6 @@ void Rover::navigate() // ---------------------------- wp_distance = get_distance(current_loc, next_WP); - if (wp_distance < 0){ - gcs_send_text(MAV_SEVERITY_CRITICAL," WP error - distance < 0"); - return; - } - // control mode specific updates to nav_bearing // -------------------------------------------- update_navigation(); diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp index 2c1ebc0179..25336ca16e 100644 --- a/APMrover2/sensors.cpp +++ b/APMrover2/sensors.cpp @@ -4,9 +4,9 @@ void Rover::init_barometer(void) { - gcs_send_text(MAV_SEVERITY_WARNING, "Calibrating barometer"); + gcs_send_text(MAV_SEVERITY_INFO, "Calibrating barometer"); barometer.calibrate(); - gcs_send_text(MAV_SEVERITY_WARNING, "barometer calibration complete"); + gcs_send_text(MAV_SEVERITY_INFO, "barometer calibration complete"); } void Rover::init_sonar(void) @@ -56,7 +56,7 @@ void Rover::read_sonars(void) obstacle.detected_count++; } if (obstacle.detected_count == g.sonar_debounce) { - gcs_send_text_fmt("Sonar1 obstacle %u cm", + gcs_send_text_fmt(MAV_SEVERITY_INFO, "Sonar1 obstacle %u cm", (unsigned)obstacle.sonar1_distance_cm); } obstacle.detected_time_ms = hal.scheduler->millis(); @@ -67,7 +67,7 @@ void Rover::read_sonars(void) obstacle.detected_count++; } if (obstacle.detected_count == g.sonar_debounce) { - gcs_send_text_fmt("Sonar2 obstacle %u cm", + gcs_send_text_fmt(MAV_SEVERITY_INFO, "Sonar2 obstacle %u cm", (unsigned)obstacle.sonar2_distance_cm); } obstacle.detected_time_ms = hal.scheduler->millis(); @@ -83,7 +83,7 @@ void Rover::read_sonars(void) obstacle.detected_count++; } if (obstacle.detected_count == g.sonar_debounce) { - gcs_send_text_fmt("Sonar obstacle %u cm", + gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Sonar obstacle %u cm", (unsigned)obstacle.sonar1_distance_cm); } obstacle.detected_time_ms = hal.scheduler->millis(); @@ -96,7 +96,7 @@ void Rover::read_sonars(void) // no object detected - reset after the turn time if (obstacle.detected_count >= g.sonar_debounce && hal.scheduler->millis() > obstacle.detected_time_ms + g.sonar_turn_time*1000) { - gcs_send_text_fmt("Obstacle passed"); + gcs_send_text_fmt(MAV_SEVERITY_INFO, "Obstacle passed"); obstacle.detected_count = 0; obstacle.turn_angle = 0; } diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 336a65af85..1feb0a2415 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -213,11 +213,11 @@ void Rover::init_ardupilot() void Rover::startup_ground(void) { set_mode(INITIALISING); - - gcs_send_text(MAV_SEVERITY_WARNING," GROUND START"); + + gcs_send_text(MAV_SEVERITY_INFO," GROUND START"); #if(GROUND_START_DELAY > 0) - gcs_send_text(MAV_SEVERITY_WARNING," With Delay"); + gcs_send_text(MAV_SEVERITY_NOTICE," With Delay"); delay(GROUND_START_DELAY * 1000); #endif @@ -241,7 +241,7 @@ void Rover::startup_ground(void) ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW)); ins.set_dataflash(&DataFlash); - gcs_send_text(MAV_SEVERITY_WARNING,"\n\n Ready to drive."); + gcs_send_text(MAV_SEVERITY_INFO,"\n\n Ready to drive."); } /* @@ -352,7 +352,7 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on) } if (failsafe.triggered != 0 && failsafe.bits == 0) { // a failsafe event has ended - gcs_send_text_fmt("Failsafe ended"); + gcs_send_text_fmt(MAV_SEVERITY_INFO, "Failsafe ended"); } failsafe.triggered &= failsafe.bits; @@ -363,7 +363,7 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on) control_mode != RTL && control_mode != HOLD) { failsafe.triggered = failsafe.bits; - gcs_send_text_fmt("Failsafe trigger 0x%x", (unsigned)failsafe.triggered); + gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Failsafe trigger 0x%x", (unsigned)failsafe.triggered); switch (g.fs_action) { case 0: break; @@ -379,12 +379,12 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on) void Rover::startup_INS_ground(void) { - gcs_send_text(MAV_SEVERITY_ALERT, "Warming up ADC..."); + gcs_send_text(MAV_SEVERITY_INFO, "Warming up ADC..."); mavlink_delay(500); // Makes the servos wiggle twice - about to begin INS calibration - HOLD LEVEL AND STILL!! // ----------------------- - gcs_send_text(MAV_SEVERITY_ALERT, "Beginning INS calibration; do not move vehicle"); + gcs_send_text(MAV_SEVERITY_INFO, "Beginning INS calibration; do not move vehicle"); mavlink_delay(1000); ahrs.init();