Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Uniformization of severity messages for Plane #3117

Closed
wants to merge 5 commits into from
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
12 changes: 6 additions & 6 deletions APMrover2/GCS_Mavlink.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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) {

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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();

Expand Down Expand Up @@ -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);
Expand Down
4 changes: 2 additions & 2 deletions APMrover2/Log.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<num_gcs; i++) {
gcs[i].reset_cli_timeout();
}
Expand Down
2 changes: 1 addition & 1 deletion APMrover2/Rover.h
Original file line number Diff line number Diff line change
Expand Up @@ -489,7 +489,7 @@ class Rover : public AP_HAL::HAL::Callbacks {
bool should_log(uint32_t mask);
void frsky_telemetry_send(void);
void print_hit_enter();
void gcs_send_text_fmt(const char *fmt, ...);
void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...);
void print_mode(AP_HAL::BetterStream *port, uint8_t mode);
bool start_command(const AP_Mission::Mission_Command& cmd);
bool verify_command(const AP_Mission::Mission_Command& cmd);
Expand Down
2 changes: 1 addition & 1 deletion APMrover2/Steering.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ bool Rover::auto_check_trigger(void)
if (!is_zero(g.auto_kickstart)) {
float xaccel = ins.get_accel().x;
if (xaccel >= 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;
}
Expand Down
4 changes: 2 additions & 2 deletions APMrover2/commands.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down Expand Up @@ -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;
Expand Down
18 changes: 9 additions & 9 deletions APMrover2/commands_logic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
}
}
Expand Down Expand Up @@ -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
Expand All @@ -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));
}
Expand All @@ -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);
}
Expand All @@ -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;
Expand Down Expand Up @@ -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());
}
}

Expand Down
5 changes: 0 additions & 5 deletions APMrover2/navigation.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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,"<navigate> WP error - distance < 0");
return;
}

// control mode specific updates to nav_bearing
// --------------------------------------------
update_navigation();
Expand Down
12 changes: 6 additions & 6 deletions APMrover2/sensors.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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();
Expand All @@ -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();
Expand All @@ -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();
Expand All @@ -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;
}
Expand Down
16 changes: 8 additions & 8 deletions APMrover2/system.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -213,11 +213,11 @@ void Rover::init_ardupilot()
void Rover::startup_ground(void)
{
set_mode(INITIALISING);

gcs_send_text(MAV_SEVERITY_WARNING,"<startup_ground> GROUND START");
gcs_send_text(MAV_SEVERITY_INFO,"<startup_ground> GROUND START");

#if(GROUND_START_DELAY > 0)
gcs_send_text(MAV_SEVERITY_WARNING,"<startup_ground> With Delay");
gcs_send_text(MAV_SEVERITY_NOTICE,"<startup_ground> With Delay");
delay(GROUND_START_DELAY * 1000);
#endif

Expand All @@ -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.");
}

/*
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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();
Expand Down
10 changes: 5 additions & 5 deletions AntennaTracker/GCS_Mavlink.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -599,7 +599,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) {

Expand Down Expand Up @@ -827,7 +827,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// check if this is the HOME wp
if (packet.seq == 0) {
tracker.set_home(tell_command); // New home in EEPROM
send_text(MAV_SEVERITY_WARNING,"new HOME received");
send_text(MAV_SEVERITY_INFO,"new HOME received");
waypoint_receiving = false;
}

Expand Down Expand Up @@ -916,7 +916,7 @@ void Tracker::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...");
}
in_mavlink_delay = false;
}
Expand Down Expand Up @@ -974,10 +974,10 @@ void Tracker::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 Tracker::gcs_send_text_fmt(const char *fmt, ...)
void Tracker::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);
Expand Down
2 changes: 1 addition & 1 deletion AntennaTracker/Tracker.h
Original file line number Diff line number Diff line change
Expand Up @@ -243,7 +243,7 @@ class Tracker : public AP_HAL::HAL::Callbacks {
void tracking_update_pressure(const mavlink_scaled_pressure_t &msg);
void tracking_manual_control(const mavlink_manual_control_t &msg);
void update_armed_disarmed();
void gcs_send_text_fmt(const char *fmt, ...);
void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...);
void init_capabilities(void);
void compass_cal_update();

Expand Down
4 changes: 2 additions & 2 deletions AntennaTracker/sensors.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -4,9 +4,9 @@

void Tracker::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");
}

// read the barometer and return the updated altitude in meters
Expand Down
2 changes: 1 addition & 1 deletion AntennaTracker/servos.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -248,7 +248,7 @@ void Tracker::update_yaw_position_servo(float yaw)
}

if (new_slew_dir != slew_dir) {
tracker.gcs_send_text_fmt("SLEW: %d/%d err=%ld servo=%ld ez=%.3f",
tracker.gcs_send_text_fmt(MAV_SEVERITY_WARNING, "SLEW: %d/%d err=%ld servo=%ld ez=%.3f",
(int)slew_dir, (int)new_slew_dir,
(long)angle_err,
(long)channel_yaw.servo_out,
Expand Down
4 changes: 2 additions & 2 deletions AntennaTracker/system.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ void Tracker::init_tracker()
if (fabsf(g.start_latitude) <= 90.0f && fabsf(g.start_longitude) <= 180.0f) {
current_loc.lat = g.start_latitude * 1.0e7f;
current_loc.lng = g.start_longitude * 1.0e7f;
gcs_send_text(MAV_SEVERITY_WARNING, "ignoring invalid START_LATITUDE or START_LONGITUDE parameter");
gcs_send_text(MAV_SEVERITY_NOTICE, "ignoring invalid START_LATITUDE or START_LONGITUDE parameter");
}

// see if EEPROM has a default location as well
Expand All @@ -104,7 +104,7 @@ void Tracker::init_tracker()

init_capabilities();

gcs_send_text(MAV_SEVERITY_WARNING,"\nReady to track.");
gcs_send_text(MAV_SEVERITY_INFO,"\nReady to track.");
hal.scheduler->delay(1000); // Why????

set_mode(AUTO); // tracking
Expand Down