mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-12 01:23:56 -03:00
Rover: Unify from print or println to printf.
This commit is contained in:
parent
577d19f91d
commit
0c813c5afa
@ -23,10 +23,10 @@ MENU2(log_menu, "Log", log_menu_commands, FUNCTOR_BIND(&rover, &Rover::print_log
|
|||||||
|
|
||||||
bool Rover::print_log_menu(void)
|
bool Rover::print_log_menu(void)
|
||||||
{
|
{
|
||||||
cliSerial->print("logs enabled: ");
|
cliSerial->printf("logs enabled: ");
|
||||||
|
|
||||||
if (0 == g.log_bitmask) {
|
if (0 == g.log_bitmask) {
|
||||||
cliSerial->print("none");
|
cliSerial->printf("none");
|
||||||
} else {
|
} else {
|
||||||
// Macro to make the following code a bit easier on the eye.
|
// Macro to make the following code a bit easier on the eye.
|
||||||
// Pass it the capitalised name of the log option, as defined
|
// Pass it the capitalised name of the log option, as defined
|
||||||
@ -50,7 +50,7 @@ bool Rover::print_log_menu(void)
|
|||||||
#undef PLOG
|
#undef PLOG
|
||||||
}
|
}
|
||||||
|
|
||||||
cliSerial->println();
|
cliSerial->printf("\n");
|
||||||
|
|
||||||
DataFlash.ListAvailableLogs(cliSerial);
|
DataFlash.ListAvailableLogs(cliSerial);
|
||||||
return(true);
|
return(true);
|
||||||
@ -69,11 +69,11 @@ int8_t Rover::dump_log(uint8_t argc, const Menu::arg *argv)
|
|||||||
DataFlash.DumpPageInfo(cliSerial);
|
DataFlash.DumpPageInfo(cliSerial);
|
||||||
return(-1);
|
return(-1);
|
||||||
} else if (dump_log_num <= 0) {
|
} else if (dump_log_num <= 0) {
|
||||||
cliSerial->println("dumping all");
|
cliSerial->printf("dumping all\n");
|
||||||
Log_Read(0, 1, 0);
|
Log_Read(0, 1, 0);
|
||||||
return(-1);
|
return(-1);
|
||||||
} else if ((argc != 2) || ((uint16_t)dump_log_num > DataFlash.get_num_logs())) {
|
} else if ((argc != 2) || ((uint16_t)dump_log_num > DataFlash.get_num_logs())) {
|
||||||
cliSerial->println("bad log number");
|
cliSerial->printf("bad log number\n");
|
||||||
return(-1);
|
return(-1);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -96,7 +96,7 @@ int8_t Rover::select_logs(uint8_t argc, const Menu::arg *argv)
|
|||||||
uint16_t bits;
|
uint16_t bits;
|
||||||
|
|
||||||
if (argc != 2) {
|
if (argc != 2) {
|
||||||
cliSerial->println("missing log type");
|
cliSerial->printf("missing log type\n");
|
||||||
return(-1);
|
return(-1);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -486,7 +486,7 @@ void Rover::Log_Read(uint16_t list_entry, uint16_t start_page, uint16_t end_page
|
|||||||
"\nFree RAM: %u\n",
|
"\nFree RAM: %u\n",
|
||||||
(unsigned)hal.util->available_memory());
|
(unsigned)hal.util->available_memory());
|
||||||
|
|
||||||
cliSerial->println(HAL_BOARD_NAME);
|
cliSerial->printf("%s\n", HAL_BOARD_NAME);
|
||||||
|
|
||||||
DataFlash.LogReadProcess(list_entry, start_page, end_page,
|
DataFlash.LogReadProcess(list_entry, start_page, end_page,
|
||||||
FUNCTOR_BIND_MEMBER(&Rover::print_mode, void, AP_HAL::BetterStream *, uint8_t),
|
FUNCTOR_BIND_MEMBER(&Rover::print_mode, void, AP_HAL::BetterStream *, uint8_t),
|
||||||
|
@ -575,19 +575,19 @@ const AP_Param::ConversionInfo conversion_table[] = {
|
|||||||
void Rover::load_parameters(void)
|
void Rover::load_parameters(void)
|
||||||
{
|
{
|
||||||
if (!AP_Param::check_var_info()) {
|
if (!AP_Param::check_var_info()) {
|
||||||
cliSerial->println("Bad var table");
|
cliSerial->printf("Bad var table\n");
|
||||||
AP_HAL::panic("Bad var table");
|
AP_HAL::panic("Bad var table");
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!g.format_version.load() ||
|
if (!g.format_version.load() ||
|
||||||
g.format_version != Parameters::k_format_version) {
|
g.format_version != Parameters::k_format_version) {
|
||||||
// erase all parameters
|
// erase all parameters
|
||||||
cliSerial->println("Firmware change: erasing EEPROM...");
|
cliSerial->printf("Firmware change: erasing EEPROM...\n");
|
||||||
AP_Param::erase_all();
|
AP_Param::erase_all();
|
||||||
|
|
||||||
// save the current format version
|
// save the current format version
|
||||||
g.format_version.set_and_save(Parameters::k_format_version);
|
g.format_version.set_and_save(Parameters::k_format_version);
|
||||||
cliSerial->println("done.");
|
cliSerial->printf("done.\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
unsigned long before = micros();
|
unsigned long before = micros();
|
||||||
|
@ -94,7 +94,7 @@ void Rover::read_trim_switch()
|
|||||||
|
|
||||||
switch (control_mode) {
|
switch (control_mode) {
|
||||||
case MANUAL:
|
case MANUAL:
|
||||||
hal.console->println("Erasing waypoints");
|
hal.console->printf("Erasing waypoints\n");
|
||||||
// if SW7 is ON in MANUAL = Erase the Flight Plan
|
// if SW7 is ON in MANUAL = Erase the Flight Plan
|
||||||
mission.clear();
|
mission.clear();
|
||||||
if (channel_steer->get_control_in() > 3000) {
|
if (channel_steer->get_control_in() > 3000) {
|
||||||
|
@ -49,7 +49,7 @@ void Rover::zero_eeprom(void)
|
|||||||
{
|
{
|
||||||
cliSerial->printf("\nErasing EEPROM\n");
|
cliSerial->printf("\nErasing EEPROM\n");
|
||||||
StorageManager::erase();
|
StorageManager::erase();
|
||||||
cliSerial->println("done");
|
cliSerial->printf("done\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // CLI_ENABLED
|
#endif // CLI_ENABLED
|
||||||
|
@ -147,7 +147,7 @@ void Rover::init_ardupilot()
|
|||||||
|
|
||||||
if (g.compass_enabled == true) {
|
if (g.compass_enabled == true) {
|
||||||
if (!compass.init()|| !compass.read()) {
|
if (!compass.init()|| !compass.read()) {
|
||||||
cliSerial->println("Compass initialisation failed!");
|
cliSerial->printf("Compass initialisation failed!\n");
|
||||||
g.compass_enabled = false;
|
g.compass_enabled = false;
|
||||||
} else {
|
} else {
|
||||||
ahrs.set_compass(&compass);
|
ahrs.set_compass(&compass);
|
||||||
@ -192,12 +192,12 @@ void Rover::init_ardupilot()
|
|||||||
//
|
//
|
||||||
if (g.cli_enabled == 1) {
|
if (g.cli_enabled == 1) {
|
||||||
const char *msg = "\nPress ENTER 3 times to start interactive setup\n";
|
const char *msg = "\nPress ENTER 3 times to start interactive setup\n";
|
||||||
cliSerial->println(msg);
|
cliSerial->printf("%s\n", msg);
|
||||||
if (gcs[1].initialised && (gcs[1].get_uart() != nullptr)) {
|
if (gcs[1].initialised && (gcs[1].get_uart() != nullptr)) {
|
||||||
gcs[1].get_uart()->println(msg);
|
gcs[1].get_uart()->printf("%s\n", msg);
|
||||||
}
|
}
|
||||||
if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != nullptr)) {
|
if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != nullptr)) {
|
||||||
gcs[2].get_uart()->println(msg);
|
gcs[2].get_uart()->printf("%s\n", msg);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
@ -443,22 +443,22 @@ void Rover::print_mode(AP_HAL::BetterStream *port, uint8_t mode)
|
|||||||
{
|
{
|
||||||
switch (mode) {
|
switch (mode) {
|
||||||
case MANUAL:
|
case MANUAL:
|
||||||
port->print("Manual");
|
port->printf("Manual");
|
||||||
break;
|
break;
|
||||||
case HOLD:
|
case HOLD:
|
||||||
port->print("HOLD");
|
port->printf("HOLD");
|
||||||
break;
|
break;
|
||||||
case LEARNING:
|
case LEARNING:
|
||||||
port->print("Learning");
|
port->printf("Learning");
|
||||||
break;
|
break;
|
||||||
case STEERING:
|
case STEERING:
|
||||||
port->print("Steering");
|
port->printf("Steering");
|
||||||
break;
|
break;
|
||||||
case AUTO:
|
case AUTO:
|
||||||
port->print("AUTO");
|
port->printf("AUTO");
|
||||||
break;
|
break;
|
||||||
case RTL:
|
case RTL:
|
||||||
port->print("RTL");
|
port->printf("RTL");
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
port->printf("Mode(%u)", (unsigned)mode);
|
port->printf("Mode(%u)", (unsigned)mode);
|
||||||
|
@ -50,13 +50,13 @@ int8_t Rover::test_passthru(uint8_t argc, const Menu::arg *argv)
|
|||||||
|
|
||||||
// New radio frame? (we could use also if((millis()- timer) > 20)
|
// New radio frame? (we could use also if((millis()- timer) > 20)
|
||||||
if (hal.rcin->new_input()) {
|
if (hal.rcin->new_input()) {
|
||||||
cliSerial->print("CH:");
|
cliSerial->printf("CH:");
|
||||||
for (int i = 0; i < 8; i++) {
|
for (int i = 0; i < 8; i++) {
|
||||||
cliSerial->print(hal.rcin->read(i)); // Print channel values
|
cliSerial->printf("%u", hal.rcin->read(i)); // Print channel values
|
||||||
cliSerial->print(",");
|
cliSerial->printf(",");
|
||||||
hal.rcout->write(i, hal.rcin->read(i)); // Copy input to Servos
|
hal.rcout->write(i, hal.rcin->read(i)); // Copy input to Servos
|
||||||
}
|
}
|
||||||
cliSerial->println();
|
cliSerial->printf("\n");
|
||||||
}
|
}
|
||||||
if (cliSerial->available() > 0){
|
if (cliSerial->available() > 0){
|
||||||
return (0);
|
return (0);
|
||||||
@ -81,7 +81,7 @@ int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv)
|
|||||||
|
|
||||||
oldSwitchPosition = readSwitch();
|
oldSwitchPosition = readSwitch();
|
||||||
|
|
||||||
cliSerial->println("Unplug battery, throttle in neutral, turn off radio.");
|
cliSerial->printf("Unplug battery, throttle in neutral, turn off radio.\n");
|
||||||
while (channel_throttle->get_control_in() > 0) {
|
while (channel_throttle->get_control_in() > 0) {
|
||||||
delay(20);
|
delay(20);
|
||||||
read_radio();
|
read_radio();
|
||||||
@ -97,16 +97,16 @@ int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv)
|
|||||||
}
|
}
|
||||||
|
|
||||||
if (oldSwitchPosition != readSwitch()){
|
if (oldSwitchPosition != readSwitch()){
|
||||||
cliSerial->print("CONTROL MODE CHANGED: ");
|
cliSerial->printf("CONTROL MODE CHANGED: ");
|
||||||
print_mode(cliSerial, readSwitch());
|
print_mode(cliSerial, readSwitch());
|
||||||
cliSerial->println();
|
cliSerial->printf("\n");
|
||||||
fail_test++;
|
fail_test++;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (throttle_failsafe_active()) {
|
if (throttle_failsafe_active()) {
|
||||||
cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", channel_throttle->get_radio_in());
|
cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", channel_throttle->get_radio_in());
|
||||||
print_mode(cliSerial, readSwitch());
|
print_mode(cliSerial, readSwitch());
|
||||||
cliSerial->println();
|
cliSerial->printf("\n");
|
||||||
fail_test++;
|
fail_test++;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -114,7 +114,7 @@ int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv)
|
|||||||
return (0);
|
return (0);
|
||||||
}
|
}
|
||||||
if (cliSerial->available() > 0) {
|
if (cliSerial->available() > 0) {
|
||||||
cliSerial->println("LOS caused no change in APM.");
|
cliSerial->printf("LOS caused no change in APM.\n");
|
||||||
return (0);
|
return (0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -126,14 +126,14 @@ int8_t Rover::test_relay(uint8_t argc, const Menu::arg *argv)
|
|||||||
delay(1000);
|
delay(1000);
|
||||||
|
|
||||||
while (1) {
|
while (1) {
|
||||||
cliSerial->println("Relay on");
|
cliSerial->printf("Relay on\n");
|
||||||
relay.on(0);
|
relay.on(0);
|
||||||
delay(3000);
|
delay(3000);
|
||||||
if (cliSerial->available() > 0) {
|
if (cliSerial->available() > 0) {
|
||||||
return (0);
|
return (0);
|
||||||
}
|
}
|
||||||
|
|
||||||
cliSerial->println("Relay off");
|
cliSerial->printf("Relay off\n");
|
||||||
relay.off(0);
|
relay.off(0);
|
||||||
delay(3000);
|
delay(3000);
|
||||||
if (cliSerial->available() > 0) {
|
if (cliSerial->available() > 0) {
|
||||||
@ -176,9 +176,9 @@ int8_t Rover::test_modeswitch(uint8_t argc, const Menu::arg *argv)
|
|||||||
print_hit_enter();
|
print_hit_enter();
|
||||||
delay(1000);
|
delay(1000);
|
||||||
|
|
||||||
cliSerial->print("Control CH ");
|
cliSerial->printf("Control CH ");
|
||||||
|
|
||||||
cliSerial->println(MODE_CHANNEL, BASE_DEC);
|
cliSerial->printf("%d\n", MODE_CHANNEL);
|
||||||
|
|
||||||
while (1) {
|
while (1) {
|
||||||
delay(20);
|
delay(20);
|
||||||
@ -198,7 +198,7 @@ int8_t Rover::test_modeswitch(uint8_t argc, const Menu::arg *argv)
|
|||||||
*/
|
*/
|
||||||
int8_t Rover::test_logging(uint8_t argc, const Menu::arg *argv)
|
int8_t Rover::test_logging(uint8_t argc, const Menu::arg *argv)
|
||||||
{
|
{
|
||||||
cliSerial->println("Testing dataflash logging");
|
cliSerial->printf("Testing dataflash logging\n");
|
||||||
DataFlash.ShowDeviceInfo(cliSerial);
|
DataFlash.ShowDeviceInfo(cliSerial);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
@ -227,7 +227,7 @@ int8_t Rover::test_gps(uint8_t argc, const Menu::arg *argv)
|
|||||||
(long)loc.alt/100,
|
(long)loc.alt/100,
|
||||||
(int)gps.num_sats());
|
(int)gps.num_sats());
|
||||||
} else {
|
} else {
|
||||||
cliSerial->print(".");
|
cliSerial->printf(".");
|
||||||
}
|
}
|
||||||
if (cliSerial->available() > 0) {
|
if (cliSerial->available() > 0) {
|
||||||
return (0);
|
return (0);
|
||||||
@ -237,7 +237,7 @@ int8_t Rover::test_gps(uint8_t argc, const Menu::arg *argv)
|
|||||||
|
|
||||||
int8_t Rover::test_ins(uint8_t argc, const Menu::arg *argv)
|
int8_t Rover::test_ins(uint8_t argc, const Menu::arg *argv)
|
||||||
{
|
{
|
||||||
// cliSerial->print("Calibrating.");
|
// cliSerial->printf("Calibrating.");
|
||||||
ahrs.init();
|
ahrs.init();
|
||||||
ahrs.set_fly_forward(true);
|
ahrs.set_fly_forward(true);
|
||||||
|
|
||||||
@ -281,23 +281,23 @@ int8_t Rover::test_ins(uint8_t argc, const Menu::arg *argv)
|
|||||||
void Rover::print_enabled(bool b)
|
void Rover::print_enabled(bool b)
|
||||||
{
|
{
|
||||||
if (b) {
|
if (b) {
|
||||||
cliSerial->print("en");
|
cliSerial->printf("en");
|
||||||
} else {
|
} else {
|
||||||
cliSerial->print("dis");
|
cliSerial->printf("dis");
|
||||||
}
|
}
|
||||||
cliSerial->println("abled");
|
cliSerial->printf("abled\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv)
|
int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv)
|
||||||
{
|
{
|
||||||
if (!g.compass_enabled) {
|
if (!g.compass_enabled) {
|
||||||
cliSerial->print("Compass: ");
|
cliSerial->printf("Compass: ");
|
||||||
print_enabled(false);
|
print_enabled(false);
|
||||||
return (0);
|
return (0);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!compass.init()) {
|
if (!compass.init()) {
|
||||||
cliSerial->println("Compass initialisation failed!");
|
cliSerial->printf("Compass initialisation failed!\n");
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
ahrs.init();
|
ahrs.init();
|
||||||
@ -340,7 +340,7 @@ int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv)
|
|||||||
(double)mag.x, (double)mag.y, (double)mag.z,
|
(double)mag.x, (double)mag.y, (double)mag.z,
|
||||||
(double)mag_ofs.x, (double)mag_ofs.y, (double)mag_ofs.z);
|
(double)mag_ofs.x, (double)mag_ofs.y, (double)mag_ofs.z);
|
||||||
} else {
|
} else {
|
||||||
cliSerial->println("compass not healthy");
|
cliSerial->printf("compass not healthy\n");
|
||||||
}
|
}
|
||||||
counter = 0;
|
counter = 0;
|
||||||
}
|
}
|
||||||
@ -351,7 +351,7 @@ int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv)
|
|||||||
|
|
||||||
// save offsets. This allows you to get sane offset values using
|
// save offsets. This allows you to get sane offset values using
|
||||||
// the CLI before you go flying.
|
// the CLI before you go flying.
|
||||||
cliSerial->println("saving offsets");
|
cliSerial->printf("saving offsets\n");
|
||||||
compass.save_offsets();
|
compass.save_offsets();
|
||||||
return (0);
|
return (0);
|
||||||
}
|
}
|
||||||
@ -366,7 +366,7 @@ int8_t Rover::test_sonar(uint8_t argc, const Menu::arg *argv)
|
|||||||
sonar.update();
|
sonar.update();
|
||||||
|
|
||||||
if (sonar.status() == RangeFinder::RangeFinder_NotConnected) {
|
if (sonar.status() == RangeFinder::RangeFinder_NotConnected) {
|
||||||
cliSerial->println("WARNING: Sonar is not enabled");
|
cliSerial->printf("WARNING: Sonar is not enabled\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
print_hit_enter();
|
print_hit_enter();
|
||||||
|
Loading…
Reference in New Issue
Block a user