Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: use send_text method on the GCS singleton

This commit is contained in:
Peter Barker 2017-07-09 11:40:59 +10:00 committed by Tom Pittenger
parent 5a8f01c19c
commit 9509f7f1bf
10 changed files with 36 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
APMrover2/APMrover2.cpp
View File

@ -458,7 +458,7 @@ void Rover::update_current_mode(void)
break;
default:
gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break;
}
break;
@ -566,7 +566,7 @@ void Rover::update_navigation()
break;
default:
gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break;
}
break;

23
APMrover2/GCS_Mavlink.cpp
View File

@ -1584,7 +1584,7 @@ void Rover::mavlink_delay_cb()
}
if (tnow - last_5s > 5000) {
last_5s = tnow;
gcs_send_text(MAV_SEVERITY_INFO, "Initialising APM");
gcs().send_text(MAV_SEVERITY_INFO, "Initialising APM");
}
check_usb_mux();
@ -1624,27 +1624,6 @@ void Rover::gcs_update(void)
gcs().update();
}
void Rover::gcs_send_text(MAV_SEVERITY severity, const char *str)
{
gcs().send_statustext(severity, 0xFF, str);
}
/*
* send a low priority formatted message to the GCS
* 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(MAV_SEVERITY severity, const char *fmt, ...)
{
char str[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] {};
va_list arg_list;
va_start(arg_list, fmt);
hal.util->vsnprintf(&str[0], sizeof(str), fmt, arg_list);
va_end(arg_list);
gcs().send_statustext(severity, 0xFF, str);
}
/**
retry any deferred messages
*/

2
APMrover2/Rover.h
View File

@ -455,7 +455,6 @@ private:
void gcs_send_mission_item_reached_message(uint16_t mission_index);
void gcs_data_stream_send(void);
void gcs_update(void);
void gcs_send_text(MAV_SEVERITY severity, const char *str);
void gcs_retry_deferred(void);
void do_erase_logs(void);
@ -558,7 +557,6 @@ private:
uint8_t check_digital_pin(uint8_t pin);
bool should_log(uint32_t mask);
void print_hit_enter();
void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...);
void print_mode(AP_HAL::BetterStream *port, uint8_t mode);
void notify_mode(enum mode new_mode);
bool start_command(const AP_Mission::Mission_Command& cmd);

6
APMrover2/Steering.cpp
View File

@ -11,7 +11,7 @@ bool Rover::auto_check_trigger(void) {
// check for user pressing the auto trigger to off
if (auto_triggered && g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 1) {
gcs_send_text(MAV_SEVERITY_WARNING, "AUTO triggered off");
gcs().send_text(MAV_SEVERITY_WARNING, "AUTO triggered off");
auto_triggered = false;
return false;
}
@ -29,7 +29,7 @@ bool Rover::auto_check_trigger(void) {
}
if (g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 0) {
gcs_send_text(MAV_SEVERITY_WARNING, "Triggered AUTO with pin");
gcs().send_text(MAV_SEVERITY_WARNING, "Triggered AUTO with pin");
auto_triggered = true;
return true;
}
@ -37,7 +37,7 @@ bool Rover::auto_check_trigger(void) {
if (!is_zero(g.auto_kickstart)) {
const float xaccel = ins.get_accel().x;
if (xaccel >= g.auto_kickstart) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Triggered AUTO xaccel=%.1f", static_cast<double>(xaccel));
gcs().send_text(MAV_SEVERITY_WARNING, "Triggered AUTO xaccel=%.1f", static_cast<double>(xaccel));
auto_triggered = true;
return true;
}

4
APMrover2/commands.cpp
View File

@ -18,7 +18,7 @@ void Rover::set_auto_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_NOTICE, "Resetting previous WP");
gcs().send_text(MAV_SEVERITY_NOTICE, "Resetting previous WP");
prev_WP = current_loc;
}
@ -139,7 +139,7 @@ bool Rover::set_home(const Location& loc, bool lock)
GCS_MAVLINK::send_home_all(loc);
// send text of home position to ground stations
rover.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %.2fm",
gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %.2fm",
static_cast<double>(loc.lat * 1.0e-7f),
static_cast<double>(loc.lng * 1.0e-7f),
static_cast<double>(loc.alt * 0.01f));

28
APMrover2/commands_logic.cpp
View File

@ -15,7 +15,7 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
return false;
}
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Executing command ID #%i", cmd.id);
gcs().send_text(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);
@ -131,7 +131,7 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
void Rover::exit_mission()
{
if (control_mode == AUTO) {
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "No commands. Can't set AUTO. Setting HOLD");
gcs().send_text(MAV_SEVERITY_NOTICE, "No commands. Can't set AUTO. Setting HOLD");
set_mode(HOLD);
}
}
@ -203,7 +203,7 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
default:
// error message
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Skipping invalid cmd #%i", cmd.id);
gcs().send_text(MAV_SEVERITY_WARNING, "Skipping invalid cmd #%i", cmd.id);
// return true if we do not recognize the command so that we move on to the next command
return true;
}
@ -266,7 +266,7 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
if (loiter_duration > 0) {
// Check if this is the first time we have reached the waypoint
if (!previously_reached_wp) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%u. Loiter for %u seconds",
gcs().send_text(MAV_SEVERITY_INFO, "Reached waypoint #%u. Loiter for %u seconds",
static_cast<uint32_t>(cmd.index),
static_cast<uint32_t>(loiter_duration));
// record the current time i.e. start timer
@ -281,7 +281,7 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
return false;
}
} else {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%u. Distance %dm",
gcs().send_text(MAV_SEVERITY_INFO, "Reached waypoint #%u. Distance %dm",
static_cast<uint32_t>(cmd.index),
static_cast<int32_t>(fabsf(get_distance(current_loc, next_WP))));
}
@ -301,7 +301,7 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
prev_WP = current_loc;
// Check if this is the first time we have reached the waypoint even though we have gone past it
if (!previously_reached_wp) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%u. Loiter for %u seconds",
gcs().send_text(MAV_SEVERITY_INFO, "Reached waypoint #%u. Loiter for %u seconds",
static_cast<uint32_t>(cmd.index),
static_cast<uint32_t>(loiter_duration));
// record the current time i.e. start timer
@ -314,7 +314,7 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
const float dist_to_wp = get_distance(current_loc, next_WP);
if (!is_equal(distance_past_wp, dist_to_wp)) {
distance_past_wp = dist_to_wp;
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Passed waypoint #%u. Distance %dm",
gcs().send_text(MAV_SEVERITY_INFO, "Passed waypoint #%u. Distance %dm",
static_cast<uint32_t>(cmd.index),
static_cast<int32_t>(fabsf(distance_past_wp)));
}
@ -336,14 +336,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_INFO, "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(MAV_SEVERITY_INFO, "Reached destination. Distance away %dm",
gcs().send_text(MAV_SEVERITY_INFO, "Reached destination. Distance away %dm",
static_cast<int32_t>(fabsf(get_distance(current_loc, next_WP))));
rtl_complete = true;
return true;
@ -365,7 +365,7 @@ bool Rover::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
{
const bool result = verify_nav_wp(cmd);
if (result) {
gcs_send_text(MAV_SEVERITY_WARNING, "Finished active loiter\n");
gcs().send_text(MAV_SEVERITY_WARNING, "Finished active loiter\n");
// if we have finished active loitering - turn it off
active_loiter = false;
}
@ -376,7 +376,7 @@ void Rover::nav_set_yaw_speed()
{
// if we haven't received a MAV_CMD_NAV_SET_YAW_SPEED message within the last 3 seconds bring the rover to a halt
if ((millis() - guided_control.msg_time_ms) > 3000) {
gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping");
gcs().send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping");
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0.0f);
lateral_acceleration = 0.0f;
@ -399,7 +399,7 @@ void Rover::nav_set_speed()
{
// if we haven't received a MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED message within the last 3 seconds bring the rover to a halt
if ((millis() - guided_control.msg_time_ms) > 3000) {
gcs_send_text(MAV_SEVERITY_WARNING, "SET_VELOCITY not recvd last 3secs, stopping");
gcs().send_text(MAV_SEVERITY_WARNING, "SET_VELOCITY not recvd last 3secs, stopping");
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0.0f);
lateral_acceleration = 0.0f;
@ -530,12 +530,12 @@ void Rover::do_change_speed(const AP_Mission::Mission_Command& cmd)
{
if (cmd.content.speed.target_ms > 0.0f) {
g.speed_cruise.set(cmd.content.speed.target_ms);
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Cruise speed: %.1f m/s", static_cast<double>(g.speed_cruise.get()));
gcs().send_text(MAV_SEVERITY_INFO, "Cruise speed: %.1f m/s", static_cast<double>(g.speed_cruise.get()));
}
if (cmd.content.speed.throttle_pct > 0.0f && cmd.content.speed.throttle_pct <= 100.0f) {
g.throttle_cruise.set(cmd.content.speed.throttle_pct);
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Cruise throttle: %.1f", static_cast<double>(g.throttle_cruise.get()));
gcs().send_text(MAV_SEVERITY_INFO, "Cruise throttle: %.1f", static_cast<double>(g.throttle_cruise.get()));
}
}

2
APMrover2/crash_check.cpp
View File

@ -36,7 +36,7 @@ void Rover::crash_check()
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
// send message to gcs
gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD");
gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD");
// change mode to hold and disarm
set_mode(HOLD);
if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) {

4
APMrover2/failsafe.cpp
View File

@ -66,7 +66,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(MAV_SEVERITY_INFO, "Failsafe ended");
gcs().send_text(MAV_SEVERITY_INFO, "Failsafe ended");
}
failsafe.triggered &= failsafe.bits;
@ -77,7 +77,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(MAV_SEVERITY_WARNING, "Failsafe trigger 0x%x", static_cast<uint32_t>(failsafe.triggered));
gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe trigger 0x%x", static_cast<uint32_t>(failsafe.triggered));
switch (g.fs_action) {
case 0:
break;

12
APMrover2/sensors.cpp
View File

@ -39,13 +39,13 @@ void Rover::compass_accumulate(void)
void Rover::init_barometer(bool full_calibration)
{
gcs_send_text(MAV_SEVERITY_INFO, "Calibrating barometer");
gcs().send_text(MAV_SEVERITY_INFO, "Calibrating barometer");
if (full_calibration) {
barometer.calibrate();
} else {
barometer.update_calibration();
}
gcs_send_text(MAV_SEVERITY_INFO, "Barometer calibration complete");
gcs().send_text(MAV_SEVERITY_INFO, "Barometer calibration complete");
}
void Rover::init_sonar(void)
@ -148,7 +148,7 @@ void Rover::read_sonars(void)
obstacle.detected_count++;
}
if (obstacle.detected_count == g.sonar_debounce) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Sonar1 obstacle %u cm",
gcs().send_text(MAV_SEVERITY_INFO, "Sonar1 obstacle %u cm",
static_cast<uint32_t>(obstacle.sonar1_distance_cm));
}
obstacle.detected_time_ms = AP_HAL::millis();
@ -159,7 +159,7 @@ void Rover::read_sonars(void)
obstacle.detected_count++;
}
if (obstacle.detected_count == g.sonar_debounce) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Sonar2 obstacle %u cm",
gcs().send_text(MAV_SEVERITY_INFO, "Sonar2 obstacle %u cm",
static_cast<uint32_t>(obstacle.sonar2_distance_cm));
}
obstacle.detected_time_ms = AP_HAL::millis();
@ -175,7 +175,7 @@ void Rover::read_sonars(void)
obstacle.detected_count++;
}
if (obstacle.detected_count == g.sonar_debounce) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Sonar obstacle %u cm",
gcs().send_text(MAV_SEVERITY_INFO, "Sonar obstacle %u cm",
static_cast<uint32_t>(obstacle.sonar1_distance_cm));
}
obstacle.detected_time_ms = AP_HAL::millis();
@ -188,7 +188,7 @@ void Rover::read_sonars(void)
// no object detected - reset after the turn time
if (obstacle.detected_count >= g.sonar_debounce &&
AP_HAL::millis() > obstacle.detected_time_ms + g.sonar_turn_time*1000) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Obstacle passed");
gcs().send_text(MAV_SEVERITY_INFO, "Obstacle passed");
obstacle.detected_count = 0;
obstacle.turn_angle = 0;
}

10
APMrover2/system.cpp
View File

@ -228,10 +228,10 @@ void Rover::startup_ground(void)
{
set_mode(INITIALISING);
gcs_send_text(MAV_SEVERITY_INFO, "<startup_ground> Ground start");
gcs().send_text(MAV_SEVERITY_INFO, "<startup_ground> Ground start");
#if(GROUND_START_DELAY > 0)
gcs_send_text(MAV_SEVERITY_NOTICE, "<startup_ground> With delay");
gcs().send_text(MAV_SEVERITY_NOTICE, "<startup_ground> With delay");
delay(GROUND_START_DELAY * 1000);
#endif
@ -258,7 +258,7 @@ void Rover::startup_ground(void)
// so set serial ports non-blocking once we are ready to drive
serial_manager.set_blocking_writes_all(false);
gcs_send_text(MAV_SEVERITY_INFO, "Ready to drive");
gcs().send_text(MAV_SEVERITY_INFO, "Ready to drive");
}
/*
@ -369,12 +369,12 @@ bool Rover::mavlink_set_mode(uint8_t mode)
void Rover::startup_INS_ground(void)
{
gcs_send_text(MAV_SEVERITY_INFO, "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_INFO, "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();