Browse Source

Replace use of print_P() with print()

master
Lucas De Marchi 9 years ago committed by Randy Mackay
parent
commit
a964ac38ec
  1. 12
      APMrover2/system.cpp
  2. 32
      ArduCopter/flight_mode.cpp
  3. 16
      ArduCopter/setup.cpp
  4. 2
      ArduCopter/system.cpp
  5. 28
      ArduPlane/system.cpp
  6. 2
      ArduPlane/test.cpp
  7. 6
      libraries/AP_Curve/AP_Curve.cpp
  8. 18
      libraries/AP_GPS/AP_GPS.cpp
  9. 2
      libraries/AP_HAL_FLYMAPLE/AnalogIn.cpp
  10. 4
      libraries/AP_InertialSensor/AP_InertialSensor.cpp
  11. 134
      libraries/AP_Mission/examples/AP_Mission_test/AP_Mission_test.cpp
  12. 8
      libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.cpp
  13. 2
      libraries/AP_PerfMon/AP_PerfMon_test/AP_PerfMon_test.cpp

12
APMrover2/system.cpp

@ -450,22 +450,22 @@ void Rover::print_mode(AP_HAL::BetterStream *port, uint8_t mode)
{ {
switch (mode) { switch (mode) {
case MANUAL: case MANUAL:
port->print_P("Manual"); port->print("Manual");
break; break;
case HOLD: case HOLD:
port->print_P("HOLD"); port->print("HOLD");
break; break;
case LEARNING: case LEARNING:
port->print_P("Learning"); port->print("Learning");
break; break;
case STEERING: case STEERING:
port->print_P("Steering"); port->print("Steering");
break; break;
case AUTO: case AUTO:
port->print_P("AUTO"); port->print("AUTO");
break; break;
case RTL: case RTL:
port->print_P("RTL"); port->print("RTL");
break; break;
default: default:
port->printf_P("Mode(%u)", (unsigned)mode); port->printf_P("Mode(%u)", (unsigned)mode);

32
ArduCopter/flight_mode.cpp

@ -315,52 +315,52 @@ void Copter::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
{ {
switch (mode) { switch (mode) {
case STABILIZE: case STABILIZE:
port->print_P("STABILIZE"); port->print("STABILIZE");
break; break;
case ACRO: case ACRO:
port->print_P("ACRO"); port->print("ACRO");
break; break;
case ALT_HOLD: case ALT_HOLD:
port->print_P("ALT_HOLD"); port->print("ALT_HOLD");
break; break;
case AUTO: case AUTO:
port->print_P("AUTO"); port->print("AUTO");
break; break;
case GUIDED: case GUIDED:
port->print_P("GUIDED"); port->print("GUIDED");
break; break;
case LOITER: case LOITER:
port->print_P("LOITER"); port->print("LOITER");
break; break;
case RTL: case RTL:
port->print_P("RTL"); port->print("RTL");
break; break;
case CIRCLE: case CIRCLE:
port->print_P("CIRCLE"); port->print("CIRCLE");
break; break;
case LAND: case LAND:
port->print_P("LAND"); port->print("LAND");
break; break;
case OF_LOITER: case OF_LOITER:
port->print_P("OF_LOITER"); port->print("OF_LOITER");
break; break;
case DRIFT: case DRIFT:
port->print_P("DRIFT"); port->print("DRIFT");
break; break;
case SPORT: case SPORT:
port->print_P("SPORT"); port->print("SPORT");
break; break;
case FLIP: case FLIP:
port->print_P("FLIP"); port->print("FLIP");
break; break;
case AUTOTUNE: case AUTOTUNE:
port->print_P("AUTOTUNE"); port->print("AUTOTUNE");
break; break;
case POSHOLD: case POSHOLD:
port->print_P("POSHOLD"); port->print("POSHOLD");
break; break;
case BRAKE: case BRAKE:
port->print_P("BRAKE"); port->print("BRAKE");
break; break;
default: default:
port->printf_P("Mode(%u)", (unsigned)mode); port->printf_P("Mode(%u)", (unsigned)mode);

16
ArduCopter/setup.cpp

@ -460,15 +460,15 @@ void Copter::report_compass()
} }
// motor compensation // motor compensation
cliSerial->print_P("Motor Comp: "); cliSerial->print("Motor Comp: ");
if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_DISABLED ) { if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_DISABLED ) {
cliSerial->print_P("Off\n"); cliSerial->print("Off\n");
}else{ }else{
if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_THROTTLE ) { if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_THROTTLE ) {
cliSerial->print_P("Throttle"); cliSerial->print("Throttle");
} }
if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_CURRENT ) { if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_CURRENT ) {
cliSerial->print_P("Current"); cliSerial->print("Current");
} }
Vector3f motor_compensation; Vector3f motor_compensation;
for (uint8_t i=0; i<compass.get_count(); i++) { for (uint8_t i=0; i<compass.get_count(); i++) {
@ -494,7 +494,7 @@ void Copter::print_blanks(int16_t num)
void Copter::print_divider(void) void Copter::print_divider(void)
{ {
for (int i = 0; i < 40; i++) { for (int i = 0; i < 40; i++) {
cliSerial->print_P("-"); cliSerial->print("-");
} }
cliSerial->println(); cliSerial->println();
} }
@ -502,10 +502,10 @@ void Copter::print_divider(void)
void Copter::print_enabled(bool b) void Copter::print_enabled(bool b)
{ {
if(b) if(b)
cliSerial->print_P("en"); cliSerial->print("en");
else else
cliSerial->print_P("dis"); cliSerial->print("dis");
cliSerial->print_P("abled\n"); cliSerial->print("abled\n");
} }
void Copter::report_version() void Copter::report_version()

2
ArduCopter/system.cpp

@ -282,7 +282,7 @@ void Copter::init_ardupilot()
// init vehicle capabilties // init vehicle capabilties
init_capabilities(); init_capabilities();
cliSerial->print_P("\nReady to FLY "); cliSerial->print("\nReady to FLY ");
// flag that initialisation has completed // flag that initialisation has completed
ap.initialised = true; ap.initialised = true;

28
ArduPlane/system.cpp

@ -636,43 +636,43 @@ void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
{ {
switch (mode) { switch (mode) {
case MANUAL: case MANUAL:
port->print_P("Manual"); port->print("Manual");
break; break;
case CIRCLE: case CIRCLE:
port->print_P("Circle"); port->print("Circle");
break; break;
case STABILIZE: case STABILIZE:
port->print_P("Stabilize"); port->print("Stabilize");
break; break;
case TRAINING: case TRAINING:
port->print_P("Training"); port->print("Training");
break; break;
case ACRO: case ACRO:
port->print_P("ACRO"); port->print("ACRO");
break; break;
case FLY_BY_WIRE_A: case FLY_BY_WIRE_A:
port->print_P("FBW_A"); port->print("FBW_A");
break; break;
case AUTOTUNE: case AUTOTUNE:
port->print_P("AUTOTUNE"); port->print("AUTOTUNE");
break; break;
case FLY_BY_WIRE_B: case FLY_BY_WIRE_B:
port->print_P("FBW_B"); port->print("FBW_B");
break; break;
case CRUISE: case CRUISE:
port->print_P("CRUISE"); port->print("CRUISE");
break; break;
case AUTO: case AUTO:
port->print_P("AUTO"); port->print("AUTO");
break; break;
case RTL: case RTL:
port->print_P("RTL"); port->print("RTL");
break; break;
case LOITER: case LOITER:
port->print_P("Loiter"); port->print("Loiter");
break; break;
case GUIDED: case GUIDED:
port->print_P("Guided"); port->print("Guided");
break; break;
default: default:
port->printf_P("Mode(%u)", (unsigned)mode); port->printf_P("Mode(%u)", (unsigned)mode);
@ -683,7 +683,7 @@ void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
#if CLI_ENABLED == ENABLED #if CLI_ENABLED == ENABLED
void Plane::print_comma(void) void Plane::print_comma(void)
{ {
cliSerial->print_P(","); cliSerial->print(",");
} }
#endif #endif

2
ArduPlane/test.cpp

@ -89,7 +89,7 @@ int8_t Plane::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_P("CH:"); cliSerial->print("CH:");
for(int16_t i = 0; i < 8; i++) { for(int16_t i = 0; i < 8; i++) {
cliSerial->print(hal.rcin->read(i)); // Print channel values cliSerial->print(hal.rcin->read(i)); // Print channel values
print_comma(); print_comma();

6
libraries/AP_Curve/AP_Curve.cpp

@ -85,11 +85,11 @@ void AP_Curve<T,SIZE>::dump_curve(AP_HAL::BetterStream* s)
{ {
s->println_P("Curve:"); s->println_P("Curve:");
for( uint8_t i = 0; i<_num_points; i++ ){ for( uint8_t i = 0; i<_num_points; i++ ){
s->print_P("x:"); s->print("x:");
s->print(_x[i]); s->print(_x[i]);
s->print_P("\ty:"); s->print("\ty:");
s->print(_y[i]); s->print(_y[i]);
s->print_P("\tslope:"); s->print("\tslope:");
s->print(_slope[i],4); s->print(_slope[i],4);
s->println(); s->println();
} }

18
libraries/AP_GPS/AP_GPS.cpp

@ -200,7 +200,7 @@ AP_GPS::detect_instance(uint8_t instance)
if (_type[instance] == GPS_TYPE_PX4) { if (_type[instance] == GPS_TYPE_PX4) {
// check for explicitely chosen PX4 GPS beforehand // check for explicitely chosen PX4 GPS beforehand
// it is not possible to autodetect it, nor does it require a real UART // it is not possible to autodetect it, nor does it require a real UART
hal.console->print_P(" PX4 "); hal.console->print(" PX4 ");
new_gps = new AP_GPS_PX4(*this, state[instance], _port[instance]); new_gps = new AP_GPS_PX4(*this, state[instance], _port[instance]);
goto found_gps; goto found_gps;
} }
@ -218,10 +218,10 @@ AP_GPS::detect_instance(uint8_t instance)
#if GPS_RTK_AVAILABLE #if GPS_RTK_AVAILABLE
// by default the sbf/trimble gps outputs no data on its port, until configured. // by default the sbf/trimble gps outputs no data on its port, until configured.
if (_type[instance] == GPS_TYPE_SBF) { if (_type[instance] == GPS_TYPE_SBF) {
hal.console->print_P(" SBF "); hal.console->print(" SBF ");
new_gps = new AP_GPS_SBF(*this, state[instance], _port[instance]); new_gps = new AP_GPS_SBF(*this, state[instance], _port[instance]);
} else if ((_type[instance] == GPS_TYPE_GSOF)) { } else if ((_type[instance] == GPS_TYPE_GSOF)) {
hal.console->print_P(" GSOF "); hal.console->print(" GSOF ");
new_gps = new AP_GPS_GSOF(*this, state[instance], _port[instance]); new_gps = new AP_GPS_GSOF(*this, state[instance], _port[instance]);
} }
#endif // GPS_RTK_AVAILABLE #endif // GPS_RTK_AVAILABLE
@ -265,23 +265,23 @@ AP_GPS::detect_instance(uint8_t instance)
if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) && if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) &&
pgm_read_dword(&_baudrates[dstate->last_baud]) >= 38400 && pgm_read_dword(&_baudrates[dstate->last_baud]) >= 38400 &&
AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) { AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
hal.console->print_P(" ublox "); hal.console->print(" ublox ");
new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance]); new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance]);
} }
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK19) && else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK19) &&
AP_GPS_MTK19::_detect(dstate->mtk19_detect_state, data)) { AP_GPS_MTK19::_detect(dstate->mtk19_detect_state, data)) {
hal.console->print_P(" MTK19 "); hal.console->print(" MTK19 ");
new_gps = new AP_GPS_MTK19(*this, state[instance], _port[instance]); new_gps = new AP_GPS_MTK19(*this, state[instance], _port[instance]);
} }
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK) && else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK) &&
AP_GPS_MTK::_detect(dstate->mtk_detect_state, data)) { AP_GPS_MTK::_detect(dstate->mtk_detect_state, data)) {
hal.console->print_P(" MTK "); hal.console->print(" MTK ");
new_gps = new AP_GPS_MTK(*this, state[instance], _port[instance]); new_gps = new AP_GPS_MTK(*this, state[instance], _port[instance]);
} }
#if GPS_RTK_AVAILABLE #if GPS_RTK_AVAILABLE
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) { AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) {
hal.console->print_P(" SBP "); hal.console->print(" SBP ");
new_gps = new AP_GPS_SBP(*this, state[instance], _port[instance]); new_gps = new AP_GPS_SBP(*this, state[instance], _port[instance]);
} }
#endif // HAL_CPU_CLASS #endif // HAL_CPU_CLASS
@ -289,7 +289,7 @@ AP_GPS::detect_instance(uint8_t instance)
// save a bit of code space on a 1280 // save a bit of code space on a 1280
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) && else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) &&
AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) { AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) {
hal.console->print_P(" SIRF "); hal.console->print(" SIRF ");
new_gps = new AP_GPS_SIRF(*this, state[instance], _port[instance]); new_gps = new AP_GPS_SIRF(*this, state[instance], _port[instance]);
} }
else if (now - dstate->detect_started_ms > (ARRAY_SIZE(_baudrates) * GPS_BAUD_TIME_MS)) { else if (now - dstate->detect_started_ms > (ARRAY_SIZE(_baudrates) * GPS_BAUD_TIME_MS)) {
@ -297,7 +297,7 @@ AP_GPS::detect_instance(uint8_t instance)
// a MTK or UBLOX which has booted in NMEA mode // a MTK or UBLOX which has booted in NMEA mode
if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_NMEA) && if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_NMEA) &&
AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) { AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
hal.console->print_P(" NMEA "); hal.console->print(" NMEA ");
new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]); new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]);
} }
} }

2
libraries/AP_HAL_FLYMAPLE/AnalogIn.cpp

@ -52,7 +52,7 @@ FLYMAPLEAnalogSource* FLYMAPLEAnalogIn::_create_channel(int16_t chnum) {
void FLYMAPLEAnalogIn::_register_channel(FLYMAPLEAnalogSource* ch) { void FLYMAPLEAnalogIn::_register_channel(FLYMAPLEAnalogSource* ch) {
if (_num_channels >= FLYMAPLE_INPUT_MAX_CHANNELS) { if (_num_channels >= FLYMAPLE_INPUT_MAX_CHANNELS) {
for(;;) { for(;;) {
hal.console->print_P( hal.console->print(
"Error: AP_HAL_FLYMAPLE::FLYMAPLEAnalogIn out of channels\r\n"); "Error: AP_HAL_FLYMAPLE::FLYMAPLEAnalogIn out of channels\r\n");
hal.scheduler->delay(1000); hal.scheduler->delay(1000);
} }

4
libraries/AP_InertialSensor/AP_InertialSensor.cpp

@ -922,7 +922,7 @@ AP_InertialSensor::_init_gyro()
AP_Notify::flags.initialising = true; AP_Notify::flags.initialising = true;
// cold start // cold start
hal.console->print_P("Init Gyro"); hal.console->print("Init Gyro");
/* /*
we do the gyro calibration with no board rotation. This avoids we do the gyro calibration with no board rotation. This avoids
@ -961,7 +961,7 @@ AP_InertialSensor::_init_gyro()
memset(diff_norm, 0, sizeof(diff_norm)); memset(diff_norm, 0, sizeof(diff_norm));
hal.console->print_P("*"); hal.console->print("*");
for (uint8_t k=0; k<num_gyros; k++) { for (uint8_t k=0; k<num_gyros; k++) {
gyro_sum[k].zero(); gyro_sum[k].zero();

134
libraries/AP_Mission/examples/AP_Mission_test/AP_Mission_test.cpp

@ -96,7 +96,7 @@ bool MissionTest::verify_cmd(const AP_Mission::Mission_Command& cmd)
// mission_complete - function that is called once the mission completes // mission_complete - function that is called once the mission completes
void MissionTest::mission_complete(void) void MissionTest::mission_complete(void)
{ {
hal.console->print_P("\nMission Complete!\n"); hal.console->print("\nMission Complete!\n");
} }
// run_mission_test - tests the stop and resume feature // run_mission_test - tests the stop and resume feature
@ -149,7 +149,7 @@ void MissionTest::run_mission_test()
print_mission(); print_mission();
// start mission // start mission
hal.console->print_P("\nRunning missions\n"); hal.console->print("\nRunning missions\n");
mission.start(); mission.start();
// update mission forever // update mission forever
@ -174,7 +174,7 @@ void MissionTest::init_mission()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #1 : take-off to 10m // Command #1 : take-off to 10m
@ -185,7 +185,7 @@ void MissionTest::init_mission()
cmd.content.location.lat = 0; cmd.content.location.lat = 0;
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #2 : first waypoint // Command #2 : first waypoint
@ -196,7 +196,7 @@ void MissionTest::init_mission()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #3 : second waypoint // Command #3 : second waypoint
@ -206,7 +206,7 @@ void MissionTest::init_mission()
cmd.content.location.lng = -1234567890; cmd.content.location.lng = -1234567890;
cmd.content.location.alt = 22; cmd.content.location.alt = 22;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #4 : do-jump to first waypoint 3 times // Command #4 : do-jump to first waypoint 3 times
@ -214,7 +214,7 @@ void MissionTest::init_mission()
cmd.content.jump.target = 2; cmd.content.jump.target = 2;
cmd.content.jump.num_times = 1; cmd.content.jump.num_times = 1;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #5 : RTL // Command #5 : RTL
@ -224,7 +224,7 @@ void MissionTest::init_mission()
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
cmd.content.location.alt = 0; cmd.content.location.alt = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
} }
@ -245,7 +245,7 @@ void MissionTest::init_mission_no_nav_commands()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #1 : "do" command // Command #1 : "do" command
@ -256,7 +256,7 @@ void MissionTest::init_mission_no_nav_commands()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #2 : "do" command // Command #2 : "do" command
@ -267,13 +267,13 @@ void MissionTest::init_mission_no_nav_commands()
cmd.content.location.lat = 0; cmd.content.location.lat = 0;
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #3 : "do" command // Command #3 : "do" command
cmd.id = MAV_CMD_DO_SET_SERVO; cmd.id = MAV_CMD_DO_SET_SERVO;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #4 : do-jump to first command 3 times // Command #4 : do-jump to first command 3 times
@ -281,7 +281,7 @@ void MissionTest::init_mission_no_nav_commands()
cmd.content.jump.target = 1; cmd.content.jump.target = 1;
cmd.content.jump.num_times = 1; cmd.content.jump.num_times = 1;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
} }
@ -302,7 +302,7 @@ void MissionTest::init_mission_endless_loop()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #1 : do-jump command to itself // Command #1 : do-jump command to itself
@ -310,7 +310,7 @@ void MissionTest::init_mission_endless_loop()
cmd.content.jump.target = 1; cmd.content.jump.target = 1;
cmd.content.jump.num_times = 2; cmd.content.jump.num_times = 2;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #2 : take-off to 10m // Command #2 : take-off to 10m
@ -321,7 +321,7 @@ void MissionTest::init_mission_endless_loop()
cmd.content.location.lat = 0; cmd.content.location.lat = 0;
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #3 : waypoint // Command #3 : waypoint
@ -332,7 +332,7 @@ void MissionTest::init_mission_endless_loop()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
} }
@ -354,7 +354,7 @@ void MissionTest::init_mission_jump_to_nonnav()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #1 : take-off to 10m // Command #1 : take-off to 10m
@ -365,7 +365,7 @@ void MissionTest::init_mission_jump_to_nonnav()
cmd.content.location.lat = 0; cmd.content.location.lat = 0;
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #2 : do-roi command // Command #2 : do-roi command
@ -376,7 +376,7 @@ void MissionTest::init_mission_jump_to_nonnav()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #3 : do-jump command to #2 // Command #3 : do-jump command to #2
@ -384,7 +384,7 @@ void MissionTest::init_mission_jump_to_nonnav()
cmd.content.jump.target = 2; cmd.content.jump.target = 2;
cmd.content.jump.num_times = 2; cmd.content.jump.num_times = 2;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #4 : waypoint // Command #4 : waypoint
@ -395,7 +395,7 @@ void MissionTest::init_mission_jump_to_nonnav()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
} }
@ -418,7 +418,7 @@ void MissionTest::init_mission_starts_with_do_commands()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #1 : First "do" command // Command #1 : First "do" command
@ -429,7 +429,7 @@ void MissionTest::init_mission_starts_with_do_commands()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #2 : Second "do" command // Command #2 : Second "do" command
@ -440,7 +440,7 @@ void MissionTest::init_mission_starts_with_do_commands()
cmd.content.location.lat = 0; cmd.content.location.lat = 0;
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #3 : take-off to 10m // Command #3 : take-off to 10m
@ -451,7 +451,7 @@ void MissionTest::init_mission_starts_with_do_commands()
cmd.content.location.lat = 0; cmd.content.location.lat = 0;
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #4 : Third "do" command // Command #4 : Third "do" command
@ -462,7 +462,7 @@ void MissionTest::init_mission_starts_with_do_commands()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #5 : waypoint // Command #5 : waypoint
@ -473,7 +473,7 @@ void MissionTest::init_mission_starts_with_do_commands()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
} }
@ -495,7 +495,7 @@ void MissionTest::init_mission_ends_with_do_commands()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #1 : take-off to 10m // Command #1 : take-off to 10m
@ -506,7 +506,7 @@ void MissionTest::init_mission_ends_with_do_commands()
cmd.content.location.lat = 0; cmd.content.location.lat = 0;
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #2 : "do" command // Command #2 : "do" command
@ -517,7 +517,7 @@ void MissionTest::init_mission_ends_with_do_commands()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #3 : waypoint // Command #3 : waypoint
@ -528,7 +528,7 @@ void MissionTest::init_mission_ends_with_do_commands()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #4 : "do" command after last nav command (but not at end of mission) // Command #4 : "do" command after last nav command (but not at end of mission)
@ -539,7 +539,7 @@ void MissionTest::init_mission_ends_with_do_commands()
cmd.content.location.lat = 0; cmd.content.location.lat = 0;
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #5 : "do" command at end of mission // Command #5 : "do" command at end of mission
@ -550,7 +550,7 @@ void MissionTest::init_mission_ends_with_do_commands()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
} }
@ -571,7 +571,7 @@ void MissionTest::init_mission_ends_with_jump_command()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #1 : take-off to 10m // Command #1 : take-off to 10m
@ -582,7 +582,7 @@ void MissionTest::init_mission_ends_with_jump_command()
cmd.content.location.lat = 0; cmd.content.location.lat = 0;
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #2 : "do" command // Command #2 : "do" command
@ -593,7 +593,7 @@ void MissionTest::init_mission_ends_with_jump_command()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #3 : waypoint // Command #3 : waypoint
@ -604,7 +604,7 @@ void MissionTest::init_mission_ends_with_jump_command()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #4 : "do" command after last nav command (but not at end of mission) // Command #4 : "do" command after last nav command (but not at end of mission)
@ -615,7 +615,7 @@ void MissionTest::init_mission_ends_with_jump_command()
cmd.content.location.lat = 0; cmd.content.location.lat = 0;
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #5 : "do" command at end of mission // Command #5 : "do" command at end of mission
@ -626,7 +626,7 @@ void MissionTest::init_mission_ends_with_jump_command()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #6 : do-jump command to #2 two times // Command #6 : do-jump command to #2 two times
@ -634,7 +634,7 @@ void MissionTest::init_mission_ends_with_jump_command()
cmd.content.jump.target = 3; cmd.content.jump.target = 3;
cmd.content.jump.num_times = 2; cmd.content.jump.num_times = 2;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
} }
@ -692,7 +692,7 @@ void MissionTest::run_resume_test()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #1 : take-off to 10m // Command #1 : take-off to 10m
@ -703,7 +703,7 @@ void MissionTest::run_resume_test()
cmd.content.location.lat = 0; cmd.content.location.lat = 0;
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #2 : first waypoint // Command #2 : first waypoint
@ -714,7 +714,7 @@ void MissionTest::run_resume_test()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #3 : second waypoint // Command #3 : second waypoint
@ -724,7 +724,7 @@ void MissionTest::run_resume_test()
cmd.content.location.lng = -1234567890; cmd.content.location.lng = -1234567890;
cmd.content.location.alt = 22; cmd.content.location.alt = 22;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #4 : do command // Command #4 : do command
@ -735,7 +735,7 @@ void MissionTest::run_resume_test()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #5 : RTL // Command #5 : RTL
@ -745,7 +745,7 @@ void MissionTest::run_resume_test()
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
cmd.content.location.alt = 0; cmd.content.location.alt = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// print current mission // print current mission
@ -798,7 +798,7 @@ void MissionTest::run_set_current_cmd_test()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #1 : take-off to 10m // Command #1 : take-off to 10m
@ -809,7 +809,7 @@ void MissionTest::run_set_current_cmd_test()
cmd.content.location.lat = 0; cmd.content.location.lat = 0;
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #2 : do command // Command #2 : do command
@ -820,7 +820,7 @@ void MissionTest::run_set_current_cmd_test()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #3 : first waypoint // Command #3 : first waypoint
@ -831,7 +831,7 @@ void MissionTest::run_set_current_cmd_test()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #4 : second waypoint // Command #4 : second waypoint
@ -841,7 +841,7 @@ void MissionTest::run_set_current_cmd_test()
cmd.content.location.lng = -1234567890; cmd.content.location.lng = -1234567890;
cmd.content.location.alt = 22; cmd.content.location.alt = 22;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #5 : do command // Command #5 : do command
@ -852,7 +852,7 @@ void MissionTest::run_set_current_cmd_test()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #6 : RTL // Command #6 : RTL
@ -862,7 +862,7 @@ void MissionTest::run_set_current_cmd_test()
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
cmd.content.location.alt = 0; cmd.content.location.alt = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// print current mission // print current mission
@ -903,7 +903,7 @@ void MissionTest::run_set_current_cmd_while_stopped_test()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #1 : take-off to 10m // Command #1 : take-off to 10m
@ -914,7 +914,7 @@ void MissionTest::run_set_current_cmd_while_stopped_test()
cmd.content.location.lat = 0; cmd.content.location.lat = 0;
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #2 : do command // Command #2 : do command
@ -925,7 +925,7 @@ void MissionTest::run_set_current_cmd_while_stopped_test()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #3 : first waypoint // Command #3 : first waypoint
@ -936,7 +936,7 @@ void MissionTest::run_set_current_cmd_while_stopped_test()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #4 : second waypoint // Command #4 : second waypoint
@ -946,7 +946,7 @@ void MissionTest::run_set_current_cmd_while_stopped_test()
cmd.content.location.lng = -1234567890; cmd.content.location.lng = -1234567890;
cmd.content.location.alt = 22; cmd.content.location.alt = 22;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #5 : do command // Command #5 : do command
@ -957,7 +957,7 @@ void MissionTest::run_set_current_cmd_while_stopped_test()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #6 : RTL // Command #6 : RTL
@ -967,7 +967,7 @@ void MissionTest::run_set_current_cmd_while_stopped_test()
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
cmd.content.location.alt = 0; cmd.content.location.alt = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// print current mission // print current mission
@ -1040,7 +1040,7 @@ void MissionTest::run_replace_cmd_test()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #1 : take-off to 10m // Command #1 : take-off to 10m
@ -1051,7 +1051,7 @@ void MissionTest::run_replace_cmd_test()
cmd.content.location.lat = 0; cmd.content.location.lat = 0;
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #2 : do command // Command #2 : do command
@ -1062,7 +1062,7 @@ void MissionTest::run_replace_cmd_test()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #3 : first waypoint // Command #3 : first waypoint
@ -1073,7 +1073,7 @@ void MissionTest::run_replace_cmd_test()
cmd.content.location.lat = 12345678; cmd.content.location.lat = 12345678;
cmd.content.location.lng = 23456789; cmd.content.location.lng = 23456789;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #4 : second waypoint // Command #4 : second waypoint
@ -1083,7 +1083,7 @@ void MissionTest::run_replace_cmd_test()
cmd.content.location.lng = -1234567890; cmd.content.location.lng = -1234567890;
cmd.content.location.alt = 22; cmd.content.location.alt = 22;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// Command #6 : RTL // Command #6 : RTL
@ -1093,7 +1093,7 @@ void MissionTest::run_replace_cmd_test()
cmd.content.location.lng = 0; cmd.content.location.lng = 0;
cmd.content.location.alt = 0; cmd.content.location.alt = 0;
if (!mission.add_cmd(cmd)) { if (!mission.add_cmd(cmd)) {
hal.console->print_P("failed to add command\n"); hal.console->print("failed to add command\n");
} }
// print current mission // print current mission

8
libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.cpp

@ -15,14 +15,14 @@ static ToshibaLED_I2C toshiba_led;
void setup(void) void setup(void)
{ {
// display welcome message // display welcome message
hal.console->print_P("Toshiba LED test ver 0.1\n"); hal.console->print("Toshiba LED test ver 0.1\n");
// initialise LED // initialise LED
toshiba_led.init(); toshiba_led.init();
// check if healthy // check if healthy
if (!toshiba_led.healthy()) { if (!toshiba_led.healthy()) {
hal.console->print_P("Failed to initialise Toshiba LED\n"); hal.console->print("Failed to initialise Toshiba LED\n");
} }
// turn on initialising notification // turn on initialising notification
@ -36,11 +36,11 @@ void setup(void)
void loop(void) void loop(void)
{ {
// blink test // blink test
//hal.console->print_P("Blink test\n"); //hal.console->print("Blink test\n");
//blink(); //blink();
/* /*
// full spectrum test // full spectrum test
hal.console->print_P("Spectrum test\n"); hal.console->print("Spectrum test\n");
full_spectrum(); full_spectrum();
*/ */

2
libraries/AP_PerfMon/AP_PerfMon_test/AP_PerfMon_test.cpp

@ -22,7 +22,7 @@ void setup()
{ {
AP_PERFMON_FUNCTION(setup) AP_PERFMON_FUNCTION(setup)
hal.console->print_P("Performance Monitor test v1.1\n"); hal.console->print("Performance Monitor test v1.1\n");
} }
void loop() void loop()

Loading…
Cancel
Save