From 20c6ffc5e30557e74314a48f22a227f2dd39cf00 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Sun, 25 Oct 2015 18:10:41 -0200 Subject: [PATCH] Replace use of UARTDriver::printf_P() with UARTDriver::printf() This also starts to show warnings on places that were already using wrong printf format strings. --- APMrover2/APMrover2.cpp | 2 +- APMrover2/Log.cpp | 18 +-- APMrover2/Parameters.cpp | 6 +- APMrover2/control_modes.cpp | 2 +- APMrover2/setup.cpp | 8 +- APMrover2/system.cpp | 6 +- APMrover2/test.cpp | 52 ++++----- AntennaTracker/Parameters.cpp | 4 +- AntennaTracker/system.cpp | 2 +- ArduCopter/Log.cpp | 42 +++---- ArduCopter/Parameters.cpp | 6 +- ArduCopter/flight_mode.cpp | 2 +- ArduCopter/setup.cpp | 110 +++++++++--------- ArduCopter/system.cpp | 4 +- ArduCopter/test.cpp | 30 ++--- ArduPlane/Log.cpp | 10 +- ArduPlane/Parameters.cpp | 6 +- ArduPlane/setup.cpp | 12 +- ArduPlane/system.cpp | 6 +- ArduPlane/test.cpp | 72 ++++++------ .../AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp | 2 +- libraries/AP_Common/AP_Common.h | 4 +- .../examples/AP_Common/AP_Common.cpp | 4 +- libraries/AP_Compass/AP_Compass_HMC5843.cpp | 22 ++-- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 2 +- .../AP_HAL/examples/AnalogIn/AnalogIn.cpp | 4 +- libraries/AP_HAL/examples/Storage/Storage.cpp | 4 +- libraries/AP_HAL_Empty/Scheduler.cpp | 2 +- libraries/AP_HAL_FLYMAPLE/Scheduler.cpp | 2 +- .../examples/Console/Console.cpp | 2 +- .../I2CDriver_HMC5883L/I2CDriver_HMC5883L.cpp | 10 +- .../examples/RCInput/RCInput.cpp | 4 +- .../RCPassthroughTest/RCPassthroughTest.cpp | 6 +- .../examples/SPIDriver/SPIDriver.cpp | 2 +- .../examples/Scheduler/Scheduler.cpp | 18 +-- .../examples/Semaphore/Semaphore.cpp | 6 +- .../examples/Storage/Storage.cpp | 18 +-- .../examples/UARTDriver/UARTDriver.cpp | 2 +- libraries/AP_HAL_SITL/Scheduler.cpp | 2 +- .../AP_InertialSensor/AP_InertialSensor.cpp | 4 +- .../AP_InertialSensor_LSM9DS0.cpp | 4 +- .../AP_InertialSensor_MPU6000.cpp | 12 +- .../AP_InertialSensor_MPU9150.cpp | 4 +- .../AP_InertialSensor_MPU9250.cpp | 2 +- .../examples/INS_generic/INS_generic.cpp | 8 +- libraries/AP_Math/examples/eulers/eulers.cpp | 4 +- .../AP_Math/examples/polygon/polygon.cpp | 2 +- .../AP_Math/examples/rotations/rotations.cpp | 2 +- libraries/AP_Math/location.cpp | 4 +- libraries/AP_Menu/AP_Menu.cpp | 4 +- .../AP_Mission_test/AP_Mission_test.cpp | 74 ++++++------ .../AP_Motors_Time_test.cpp | 4 +- .../AP_Motors_test/AP_Motors_test.cpp | 6 +- .../AP_OpticalFlow/AP_OpticalFlow_Linux.cpp | 2 +- .../AP_Parachute_test/AP_Parachute_test.cpp | 2 +- libraries/AP_Param/AP_Param.cpp | 14 +-- libraries/AP_PerfMon/AP_PerfMon.cpp | 8 +- .../examples/RFIND_test/RFIND_test.cpp | 6 +- libraries/AP_Scheduler/AP_Scheduler.cpp | 4 +- libraries/DataFlash/DataFlash_APM1.cpp | 2 +- libraries/DataFlash/DataFlash_File.cpp | 10 +- libraries/DataFlash/LogFile.cpp | 58 ++++----- .../DataFlash_test/DataFlash_test.cpp | 2 +- .../GCS_Console/examples/Console/Console.cpp | 2 +- .../examples/Console/simplegcs.cpp | 2 +- libraries/PID/examples/pid/pid.cpp | 2 +- 66 files changed, 382 insertions(+), 382 deletions(-) diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index ea5fa76d75..e2cab03d27 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -302,7 +302,7 @@ void Rover::one_second_loop(void) // write perf data every 20s if (counter % 10 == 0) { if (scheduler.debug() != 0) { - hal.console->printf_P("G_Dt_max=%lu\n", (unsigned long)G_Dt_max); + hal.console->printf("G_Dt_max=%lu\n", (unsigned long)G_Dt_max); } if (should_log(MASK_LOG_PM)) Log_Write_Performance(); diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index d79a1be4f2..ac3229a440 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -24,16 +24,16 @@ MENU2(log_menu, "Log", log_menu_commands, FUNCTOR_BIND(&rover, &Rover::print_log bool Rover::print_log_menu(void) { - cliSerial->printf_P("logs enabled: "); + cliSerial->printf("logs enabled: "); if (0 == g.log_bitmask) { - cliSerial->printf_P("none"); + cliSerial->printf("none"); }else{ // Macro to make the following code a bit easier on the eye. // Pass it the capitalised name of the log option, as defined // in defines.h but without the LOG_ prefix. It will check for // the bit being set and print the name of the log option to suit. - #define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf_P(" %S", #_s) + #define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf(" %S", #_s) PLOG(ATTITUDE_FAST); PLOG(ATTITUDE_MED); PLOG(GPS); @@ -70,11 +70,11 @@ int8_t Rover::dump_log(uint8_t argc, const Menu::arg *argv) DataFlash.DumpPageInfo(cliSerial); return(-1); } else if (dump_log_num <= 0) { - cliSerial->printf_P("dumping all\n"); + cliSerial->printf("dumping all\n"); Log_Read(0, 1, 0); return(-1); } else if ((argc != 2) || ((uint16_t)dump_log_num > DataFlash.get_num_logs())) { - cliSerial->printf_P("bad log number\n"); + cliSerial->printf("bad log number\n"); return(-1); } @@ -97,7 +97,7 @@ int8_t Rover::select_logs(uint8_t argc, const Menu::arg *argv) uint16_t bits; if (argc != 2) { - cliSerial->printf_P("missing log type\n"); + cliSerial->printf("missing log type\n"); return(-1); } @@ -148,9 +148,9 @@ int8_t Rover::process_logs(uint8_t argc, const Menu::arg *argv) void Rover::do_erase_logs(void) { - cliSerial->printf_P("\nErasing log...\n"); + cliSerial->printf("\nErasing log...\n"); DataFlash.EraseAll(); - cliSerial->printf_P("\nLog erased.\n"); + cliSerial->printf("\nLog erased.\n"); } @@ -411,7 +411,7 @@ void Rover::log_init(void) // Read the DataFlash log memory : Packet Parser void Rover::Log_Read(uint16_t list_entry, uint16_t start_page, uint16_t end_page) { - cliSerial->printf_P("\n" FIRMWARE_STRING + cliSerial->printf("\n" FIRMWARE_STRING "\nFree RAM: %u\n", (unsigned)hal.util->available_memory()); diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index a62e536f0f..a7298a12a3 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -566,7 +566,7 @@ const AP_Param::ConversionInfo conversion_table[] = { void Rover::load_parameters(void) { if (!AP_Param::check_var_info()) { - cliSerial->printf_P("Bad var table\n"); + cliSerial->printf("Bad var table\n"); hal.scheduler->panic("Bad var table"); } @@ -574,7 +574,7 @@ void Rover::load_parameters(void) g.format_version != Parameters::k_format_version) { // erase all parameters - cliSerial->printf_P("Firmware change: erasing EEPROM...\n"); + cliSerial->printf("Firmware change: erasing EEPROM...\n"); AP_Param::erase_all(); // save the current format version @@ -585,7 +585,7 @@ void Rover::load_parameters(void) // Load all auto-loaded EEPROM variables AP_Param::load_all(); - cliSerial->printf_P("load_all took %luus\n", micros() - before); + cliSerial->printf("load_all took %luus\n", micros() - before); } // set a more reasonable default NAVL1_PERIOD for rovers diff --git a/APMrover2/control_modes.cpp b/APMrover2/control_modes.cpp index b95576be1a..e2940e7eea 100644 --- a/APMrover2/control_modes.cpp +++ b/APMrover2/control_modes.cpp @@ -105,7 +105,7 @@ void Rover::read_trim_switch() // save command if(mission.add_cmd(cmd)) { - hal.console->printf_P("Learning waypoint %u", (unsigned)mission.num_commands()); + hal.console->printf("Learning waypoint %u", (unsigned)mission.num_commands()); } } else if (control_mode == AUTO) { // if SW7 is ON in AUTO = set to RTL diff --git a/APMrover2/setup.cpp b/APMrover2/setup.cpp index 78d6038f4f..3fcba92687 100644 --- a/APMrover2/setup.cpp +++ b/APMrover2/setup.cpp @@ -18,7 +18,7 @@ MENU(setup_menu, "setup", setup_menu_commands); int8_t Rover::setup_mode(uint8_t argc, const Menu::arg *argv) { // Give the user some guidance - cliSerial->printf_P("Setup Mode\n" + cliSerial->printf("Setup Mode\n" "\n" "IMPORTANT: if you have not previously set this system up, use the\n" "'reset' command to initialize the EEPROM to sensible default values\n" @@ -34,7 +34,7 @@ int8_t Rover::setup_erase(uint8_t argc, const Menu::arg *argv) { int c; - cliSerial->printf_P("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "); + cliSerial->printf("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "); do { c = cliSerial->read(); @@ -48,9 +48,9 @@ int8_t Rover::setup_erase(uint8_t argc, const Menu::arg *argv) void Rover::zero_eeprom(void) { - cliSerial->printf_P("\nErasing EEPROM\n"); + cliSerial->printf("\nErasing EEPROM\n"); StorageManager::erase(); - cliSerial->printf_P("done\n"); + cliSerial->printf("done\n"); } #endif // CLI_ENABLED diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index f5d9fd480f..763982279f 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -13,7 +13,7 @@ The init_ardupilot function processes everything we need for an in - air restart // This is the help function int8_t Rover::main_menu_help(uint8_t argc, const Menu::arg *argv) { - cliSerial->printf_P("Commands:\n" + cliSerial->printf("Commands:\n" " logs log readback/setup mode\n" " setup setup mode\n" " test test mode\n" @@ -83,7 +83,7 @@ void Rover::init_ardupilot() // initialise console serial port serial_manager.init_console(); - cliSerial->printf_P("\n\nInit " FIRMWARE_STRING + cliSerial->printf("\n\nInit " FIRMWARE_STRING "\n\nFree RAM: %u\n", hal.util->available_memory()); @@ -468,7 +468,7 @@ void Rover::print_mode(AP_HAL::BetterStream *port, uint8_t mode) port->print("RTL"); break; default: - port->printf_P("Mode(%u)", (unsigned)mode); + port->printf("Mode(%u)", (unsigned)mode); break; } } diff --git a/APMrover2/test.cpp b/APMrover2/test.cpp index f1e4c14898..28be21fda7 100644 --- a/APMrover2/test.cpp +++ b/APMrover2/test.cpp @@ -34,14 +34,14 @@ MENU(test_menu, "test", test_menu_commands); int8_t Rover::test_mode(uint8_t argc, const Menu::arg *argv) { - cliSerial->printf_P("Test Mode\n\n"); + cliSerial->printf("Test Mode\n\n"); test_menu.run(); return 0; } void Rover::print_hit_enter() { - cliSerial->printf_P("Hit Enter to exit.\n\n"); + cliSerial->printf("Hit Enter to exit.\n\n"); } int8_t Rover::test_radio_pwm(uint8_t argc, const Menu::arg *argv) @@ -56,7 +56,7 @@ int8_t Rover::test_radio_pwm(uint8_t argc, const Menu::arg *argv) // ---------------------------------------------------------- read_radio(); - cliSerial->printf_P("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n", + cliSerial->printf("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n", channel_steer->radio_in, g.rc_2.radio_in, channel_throttle->radio_in, @@ -118,7 +118,7 @@ int8_t Rover::test_radio(uint8_t argc, const Menu::arg *argv) // ------------------------------ set_servos(); - cliSerial->printf_P("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n", + cliSerial->printf("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n", channel_steer->control_in, g.rc_2.control_in, channel_throttle->control_in, @@ -149,7 +149,7 @@ int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv) oldSwitchPosition = readSwitch(); - cliSerial->printf_P("Unplug battery, throttle in neutral, turn off radio.\n"); + cliSerial->printf("Unplug battery, throttle in neutral, turn off radio.\n"); while(channel_throttle->control_in > 0){ delay(20); read_radio(); @@ -160,19 +160,19 @@ int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv) read_radio(); if(channel_throttle->control_in > 0){ - cliSerial->printf_P("THROTTLE CHANGED %d \n", channel_throttle->control_in); + cliSerial->printf("THROTTLE CHANGED %d \n", channel_throttle->control_in); fail_test++; } if (oldSwitchPosition != readSwitch()){ - cliSerial->printf_P("CONTROL MODE CHANGED: "); + cliSerial->printf("CONTROL MODE CHANGED: "); print_mode(cliSerial, readSwitch()); cliSerial->println(); fail_test++; } if(throttle_failsafe_active()) { - cliSerial->printf_P("THROTTLE FAILSAFE ACTIVATED: %d, ", channel_throttle->radio_in); + cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", channel_throttle->radio_in); print_mode(cliSerial, readSwitch()); cliSerial->println(); fail_test++; @@ -182,7 +182,7 @@ int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv) return (0); } if(cliSerial->available() > 0){ - cliSerial->printf_P("LOS caused no change in APM.\n"); + cliSerial->printf("LOS caused no change in APM.\n"); return (0); } } @@ -194,14 +194,14 @@ int8_t Rover::test_relay(uint8_t argc, const Menu::arg *argv) delay(1000); while(1){ - cliSerial->printf_P("Relay on\n"); + cliSerial->printf("Relay on\n"); relay.on(0); delay(3000); if(cliSerial->available() > 0){ return (0); } - cliSerial->printf_P("Relay off\n"); + cliSerial->printf("Relay off\n"); relay.off(0); delay(3000); if(cliSerial->available() > 0){ @@ -214,8 +214,8 @@ int8_t Rover::test_wp(uint8_t argc, const Menu::arg *argv) { delay(1000); - cliSerial->printf_P("%u waypoints\n", (unsigned)mission.num_commands()); - cliSerial->printf_P("Hit radius: %f\n", g.waypoint_radius); + cliSerial->printf("%u waypoints\n", (unsigned)mission.num_commands()); + cliSerial->printf("Hit radius: %f\n", g.waypoint_radius); for(uint8_t i = 0; i < mission.num_commands(); i++){ AP_Mission::Mission_Command temp_cmd; @@ -229,7 +229,7 @@ int8_t Rover::test_wp(uint8_t argc, const Menu::arg *argv) void Rover::test_wp_print(const AP_Mission::Mission_Command& cmd) { - cliSerial->printf_P("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n", + cliSerial->printf("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n", (int)cmd.index, (int)cmd.id, (int)cmd.content.location.options, @@ -244,7 +244,7 @@ int8_t Rover::test_modeswitch(uint8_t argc, const Menu::arg *argv) print_hit_enter(); delay(1000); - cliSerial->printf_P("Control CH "); + cliSerial->printf("Control CH "); cliSerial->println(MODE_CHANNEL, BASE_DEC); @@ -252,7 +252,7 @@ int8_t Rover::test_modeswitch(uint8_t argc, const Menu::arg *argv) delay(20); uint8_t switchPosition = readSwitch(); if (oldSwitchPosition != switchPosition){ - cliSerial->printf_P("Position %d\n", switchPosition); + cliSerial->printf("Position %d\n", switchPosition); oldSwitchPosition = switchPosition; } if(cliSerial->available() > 0){ @@ -289,13 +289,13 @@ int8_t Rover::test_gps(uint8_t argc, const Menu::arg *argv) if (gps.last_message_time_ms() != last_message_time_ms) { last_message_time_ms = gps.last_message_time_ms(); const Location &loc = gps.location(); - cliSerial->printf_P("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n", + cliSerial->printf("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n", (long)loc.lat, (long)loc.lng, (long)loc.alt/100, (int)gps.num_sats()); } else { - cliSerial->printf_P("."); + cliSerial->printf("."); } if(cliSerial->available() > 0) { return (0); @@ -305,7 +305,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) { - //cliSerial->printf_P("Calibrating."); + //cliSerial->printf("Calibrating."); ahrs.init(); ahrs.set_fly_forward(true); ins.init(ins_sample_rate); @@ -333,7 +333,7 @@ int8_t Rover::test_ins(uint8_t argc, const Menu::arg *argv) // --------------------- Vector3f gyros = ins.get_gyro(); Vector3f accels = ins.get_accel(); - cliSerial->printf_P("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n", + cliSerial->printf("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n", (int)ahrs.roll_sensor / 100, (int)ahrs.pitch_sensor / 100, (uint16_t)ahrs.yaw_sensor / 100, @@ -348,16 +348,16 @@ int8_t Rover::test_ins(uint8_t argc, const Menu::arg *argv) void Rover::print_enabled(bool b) { if(b) - cliSerial->printf_P("en"); + cliSerial->printf("en"); else - cliSerial->printf_P("dis"); - cliSerial->printf_P("abled\n"); + cliSerial->printf("dis"); + cliSerial->printf("abled\n"); } int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv) { if (!g.compass_enabled) { - cliSerial->printf_P("Compass: "); + cliSerial->printf("Compass: "); print_enabled(false); return (0); } @@ -401,7 +401,7 @@ int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv) if (compass.healthy()) { const Vector3f mag_ofs = compass.get_offsets(); const Vector3f mag = compass.get_field(); - cliSerial->printf_P("Heading: %ld, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n", + cliSerial->printf("Heading: %ld, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n", (wrap_360_cd(ToDeg(heading) * 100)) /100, (double)mag.x, (double)mag.y, (double)mag.z, (double)mag_ofs.x, (double)mag_ofs.y, (double)mag_ofs.z); @@ -473,7 +473,7 @@ int8_t Rover::test_sonar(uint8_t argc, const Menu::arg *argv) voltage2_max = max(voltage2_max, voltage); if (now - last_print >= 200) { - cliSerial->printf_P("sonar1 dist=%.1f:%.1fcm volt1=%.2f:%.2f sonar2 dist=%.1f:%.1fcm volt2=%.2f:%.2f\n", + cliSerial->printf("sonar1 dist=%.1f:%.1fcm volt1=%.2f:%.2f sonar2 dist=%.1f:%.1fcm volt2=%.2f:%.2f\n", (double)sonar_dist_cm_min, (double)sonar_dist_cm_max, (double)voltage_min, diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp index fe22b5fb56..3301005773 100644 --- a/AntennaTracker/Parameters.cpp +++ b/AntennaTracker/Parameters.cpp @@ -288,7 +288,7 @@ void Tracker::load_parameters(void) g.format_version != Parameters::k_format_version) { // erase all parameters - hal.console->printf_P("Firmware change: erasing EEPROM...\n"); + hal.console->printf("Firmware change: erasing EEPROM...\n"); AP_Param::erase_all(); // save the current format version @@ -298,6 +298,6 @@ void Tracker::load_parameters(void) uint32_t before = hal.scheduler->micros(); // Load all auto-loaded EEPROM variables AP_Param::load_all(); - hal.console->printf_P("load_all took %luus\n", (unsigned long)(hal.scheduler->micros() - before)); + hal.console->printf("load_all took %luus\n", (unsigned long)(hal.scheduler->micros() - before)); } } diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index 89bfb7bf57..b221357930 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -20,7 +20,7 @@ void Tracker::init_tracker() // initialise console serial port serial_manager.init_console(); - hal.console->printf_P("\n\nInit " THISFIRMWARE + hal.console->printf("\n\nInit " THISFIRMWARE "\n\nFree RAM: %u\n", hal.util->available_memory()); diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index b6a463bb78..c2f61c3ca4 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -25,26 +25,26 @@ MENU2(log_menu, "Log", log_menu_commands, FUNCTOR_BIND(&copter, &Copter::print_l bool Copter::print_log_menu(void) { - cliSerial->printf_P("logs enabled: "); + cliSerial->printf("logs enabled: "); if (0 == g.log_bitmask) { - cliSerial->printf_P("none"); + cliSerial->printf("none"); }else{ - if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) cliSerial->printf_P(" ATTITUDE_FAST"); - if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) cliSerial->printf_P(" ATTITUDE_MED"); - if (g.log_bitmask & MASK_LOG_GPS) cliSerial->printf_P(" GPS"); - if (g.log_bitmask & MASK_LOG_PM) cliSerial->printf_P(" PM"); - if (g.log_bitmask & MASK_LOG_CTUN) cliSerial->printf_P(" CTUN"); - if (g.log_bitmask & MASK_LOG_NTUN) cliSerial->printf_P(" NTUN"); - if (g.log_bitmask & MASK_LOG_RCIN) cliSerial->printf_P(" RCIN"); - if (g.log_bitmask & MASK_LOG_IMU) cliSerial->printf_P(" IMU"); - if (g.log_bitmask & MASK_LOG_CMD) cliSerial->printf_P(" CMD"); - if (g.log_bitmask & MASK_LOG_CURRENT) cliSerial->printf_P(" CURRENT"); - if (g.log_bitmask & MASK_LOG_RCOUT) cliSerial->printf_P(" RCOUT"); - if (g.log_bitmask & MASK_LOG_OPTFLOW) cliSerial->printf_P(" OPTFLOW"); - if (g.log_bitmask & MASK_LOG_COMPASS) cliSerial->printf_P(" COMPASS"); - if (g.log_bitmask & MASK_LOG_CAMERA) cliSerial->printf_P(" CAMERA"); - if (g.log_bitmask & MASK_LOG_PID) cliSerial->printf_P(" PID"); + if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) cliSerial->printf(" ATTITUDE_FAST"); + if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) cliSerial->printf(" ATTITUDE_MED"); + if (g.log_bitmask & MASK_LOG_GPS) cliSerial->printf(" GPS"); + if (g.log_bitmask & MASK_LOG_PM) cliSerial->printf(" PM"); + if (g.log_bitmask & MASK_LOG_CTUN) cliSerial->printf(" CTUN"); + if (g.log_bitmask & MASK_LOG_NTUN) cliSerial->printf(" NTUN"); + if (g.log_bitmask & MASK_LOG_RCIN) cliSerial->printf(" RCIN"); + if (g.log_bitmask & MASK_LOG_IMU) cliSerial->printf(" IMU"); + if (g.log_bitmask & MASK_LOG_CMD) cliSerial->printf(" CMD"); + if (g.log_bitmask & MASK_LOG_CURRENT) cliSerial->printf(" CURRENT"); + if (g.log_bitmask & MASK_LOG_RCOUT) cliSerial->printf(" RCOUT"); + if (g.log_bitmask & MASK_LOG_OPTFLOW) cliSerial->printf(" OPTFLOW"); + if (g.log_bitmask & MASK_LOG_COMPASS) cliSerial->printf(" COMPASS"); + if (g.log_bitmask & MASK_LOG_CAMERA) cliSerial->printf(" CAMERA"); + if (g.log_bitmask & MASK_LOG_PID) cliSerial->printf(" PID"); } cliSerial->println(); @@ -68,11 +68,11 @@ int8_t Copter::dump_log(uint8_t argc, const Menu::arg *argv) DataFlash.DumpPageInfo(cliSerial); return(-1); } else if (dump_log_num <= 0) { - cliSerial->printf_P("dumping all\n"); + cliSerial->printf("dumping all\n"); Log_Read(0, 1, 0); return(-1); } else if ((argc != 2) || ((uint16_t)dump_log_num > DataFlash.get_num_logs())) { - cliSerial->printf_P("bad log number\n"); + cliSerial->printf("bad log number\n"); return(-1); } @@ -95,7 +95,7 @@ int8_t Copter::select_logs(uint8_t argc, const Menu::arg *argv) uint16_t bits; if (argc != 2) { - cliSerial->printf_P("missing log type\n"); + cliSerial->printf("missing log type\n"); return(-1); } @@ -767,7 +767,7 @@ const struct LogStructure Copter::log_structure[] = { // Read the DataFlash log memory void Copter::Log_Read(uint16_t list_entry, uint16_t start_page, uint16_t end_page) { - cliSerial->printf_P("\n" FIRMWARE_STRING + cliSerial->printf("\n" FIRMWARE_STRING "\nFree RAM: %u\n" "\nFrame: " FRAME_CONFIG_STRING "\n", (unsigned) hal.util->available_memory()); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index f7bec077ca..5e71d30603 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1148,7 +1148,7 @@ const AP_Param::ConversionInfo conversion_table[] = { void Copter::load_parameters(void) { if (!AP_Param::check_var_info()) { - cliSerial->printf_P("Bad var table\n"); + cliSerial->printf("Bad var table\n"); hal.scheduler->panic("Bad var table"); } @@ -1160,7 +1160,7 @@ void Copter::load_parameters(void) g.format_version != Parameters::k_format_version) { // erase all parameters - cliSerial->printf_P("Firmware change: erasing EEPROM...\n"); + cliSerial->printf("Firmware change: erasing EEPROM...\n"); AP_Param::erase_all(); // save the current format version @@ -1171,6 +1171,6 @@ void Copter::load_parameters(void) // Load all auto-loaded EEPROM variables AP_Param::load_all(); AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table)); - cliSerial->printf_P("load_all took %luus\n", micros() - before); + cliSerial->printf("load_all took %luus\n", micros() - before); } } diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index 2cb91b3069..d6e623d3da 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -363,7 +363,7 @@ void Copter::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) port->print("BRAKE"); break; default: - port->printf_P("Mode(%u)", (unsigned)mode); + port->printf("Mode(%u)", (unsigned)mode); break; } } diff --git a/ArduCopter/setup.cpp b/ArduCopter/setup.cpp index 587755b9f9..4b8c379d9c 100644 --- a/ArduCopter/setup.cpp +++ b/ArduCopter/setup.cpp @@ -25,7 +25,7 @@ MENU(setup_menu, "setup", setup_menu_commands); int8_t Copter::setup_mode(uint8_t argc, const Menu::arg *argv) { // Give the user some guidance - cliSerial->printf_P("Setup Mode\n\n\n"); + cliSerial->printf("Setup Mode\n\n\n"); // Run the setup menu. When the menu exits, we will return to the main menu. setup_menu.run(); @@ -38,7 +38,7 @@ int8_t Copter::setup_factory(uint8_t argc, const Menu::arg *argv) { int16_t c; - cliSerial->printf_P("\n'Y' = factory reset, any other key to abort:\n"); + cliSerial->printf("\n'Y' = factory reset, any other key to abort:\n"); do { c = cliSerial->read(); @@ -48,7 +48,7 @@ int8_t Copter::setup_factory(uint8_t argc, const Menu::arg *argv) return(-1); AP_Param::erase_all(); - cliSerial->printf_P("\nReboot board"); + cliSerial->printf("\nReboot board"); delay(1000); @@ -70,14 +70,14 @@ int8_t Copter::setup_set(uint8_t argc, const Menu::arg *argv) if(argc!=3) { - cliSerial->printf_P("Invalid command. Usage: set \n"); + cliSerial->printf("Invalid command. Usage: set \n"); return 0; } param = AP_Param::find(argv[1].str, &p_type); if(!param) { - cliSerial->printf_P("Param not found: %s\n", argv[1].str); + cliSerial->printf("Param not found: %s\n", argv[1].str); return 0; } @@ -87,7 +87,7 @@ int8_t Copter::setup_set(uint8_t argc, const Menu::arg *argv) value_int8 = (int8_t)(argv[2].i); if(argv[2].i!=value_int8) { - cliSerial->printf_P("Value out of range for type INT8\n"); + cliSerial->printf("Value out of range for type INT8\n"); return 0; } ((AP_Int8*)param)->set_and_save(value_int8); @@ -96,7 +96,7 @@ int8_t Copter::setup_set(uint8_t argc, const Menu::arg *argv) value_int16 = (int16_t)(argv[2].i); if(argv[2].i!=value_int16) { - cliSerial->printf_P("Value out of range for type INT16\n"); + cliSerial->printf("Value out of range for type INT16\n"); return 0; } ((AP_Int16*)param)->set_and_save(value_int16); @@ -110,7 +110,7 @@ int8_t Copter::setup_set(uint8_t argc, const Menu::arg *argv) ((AP_Float*)param)->set_and_save(argv[2].f); break; default: - cliSerial->printf_P("Cannot set parameter of type %d.\n", p_type); + cliSerial->printf("Cannot set parameter of type %d.\n", p_type); break; } @@ -132,7 +132,7 @@ int8_t Copter::setup_show(uint8_t argc, const Menu::arg *argv) if(!param) { - cliSerial->printf_P("Parameter not found: '%s'\n", argv[1]); + cliSerial->printf("Parameter not found: '%s'\n", argv[1]); return 0; } AP_Param::show(param, argv[1].str, type, cliSerial); @@ -177,7 +177,7 @@ int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv) if (argc < 2) { - cliSerial->printf_P("Pls provide Channel Mask\n" + cliSerial->printf("Pls provide Channel Mask\n" "\tusage: esc_calib 1010 - enables calibration for 2nd and 4th Motor\n"); return(0); } @@ -186,14 +186,14 @@ int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv) set_mask = strtol (argv[1].str, NULL, 2); if (set_mask == 0) - cliSerial->printf_P("no channels chosen"); - //cliSerial->printf_P("\n%d\n",set_mask); + cliSerial->printf("no channels chosen"); + //cliSerial->printf("\n%d\n",set_mask); set_mask<<=1; /* wait 50 ms */ hal.scheduler->delay(50); - cliSerial->printf_P("\nATTENTION, please remove or fix propellers before starting calibration!\n" + cliSerial->printf("\nATTENTION, please remove or fix propellers before starting calibration!\n" "\n" "Make sure\n" "\t - that the ESCs are not powered\n" @@ -210,11 +210,11 @@ int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv) break; } else if (c == 0x03 || c == 0x63 || c == 'q') { - cliSerial->printf_P("ESC calibration exited\n"); + cliSerial->printf("ESC calibration exited\n"); return(0); } else if (c == 'n' || c == 'N') { - cliSerial->printf_P("ESC calibration aborted\n"); + cliSerial->printf("ESC calibration aborted\n"); return(0); } @@ -231,11 +231,11 @@ int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv) motors.armed(true); - cliSerial->printf_P("Outputs armed\n"); + cliSerial->printf("Outputs armed\n"); /* wait for user confirmation */ - cliSerial->printf_P("\nHigh PWM set: %d\n" + cliSerial->printf("\nHigh PWM set: %d\n" "\n" "Connect battery now and hit c+ENTER after the ESCs confirm the first calibration step\n" "\n", pwm_high); @@ -254,7 +254,7 @@ int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv) break; } else if (c == 0x03 || c == 0x63 || c == 'q') { - cliSerial->printf_P("ESC calibration exited\n"); + cliSerial->printf("ESC calibration exited\n"); return(0); } @@ -262,7 +262,7 @@ int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv) hal.scheduler->delay(50); } - cliSerial->printf_P("Low PWM set: %d\n" + cliSerial->printf("Low PWM set: %d\n" "\n" "Hit c+Enter when finished\n" "\n", pwm_low); @@ -282,7 +282,7 @@ int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv) break; } else if (c == 0x03 || c == 0x63 || c == 'q') { - cliSerial->printf_P("ESC calibration exited\n"); + cliSerial->printf("ESC calibration exited\n"); return(0); } @@ -293,9 +293,9 @@ int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv) /* disarm */ motors.armed(false); - cliSerial->printf_P("Outputs disarmed\n"); + cliSerial->printf("Outputs disarmed\n"); - cliSerial->printf_P("ESC calibration finished\n"); + cliSerial->printf("ESC calibration finished\n"); return(0); } @@ -308,35 +308,35 @@ int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv) void Copter::report_batt_monitor() { - cliSerial->printf_P("\nBatt Mon:\n"); + cliSerial->printf("\nBatt Mon:\n"); print_divider(); if (battery.num_instances() == 0) { print_enabled(false); } else if (!battery.has_current()) { - cliSerial->printf_P("volts"); + cliSerial->printf("volts"); } else { - cliSerial->printf_P("volts and cur"); + cliSerial->printf("volts and cur"); } print_blanks(2); } void Copter::report_frame() { - cliSerial->printf_P("Frame\n"); + cliSerial->printf("Frame\n"); print_divider(); #if FRAME_CONFIG == QUAD_FRAME - cliSerial->printf_P("Quad frame\n"); + cliSerial->printf("Quad frame\n"); #elif FRAME_CONFIG == TRI_FRAME - cliSerial->printf_P("TRI frame\n"); + cliSerial->printf("TRI frame\n"); #elif FRAME_CONFIG == HEXA_FRAME - cliSerial->printf_P("Hexa frame\n"); + cliSerial->printf("Hexa frame\n"); #elif FRAME_CONFIG == Y6_FRAME - cliSerial->printf_P("Y6 frame\n"); + cliSerial->printf("Y6 frame\n"); #elif FRAME_CONFIG == OCTA_FRAME - cliSerial->printf_P("Octa frame\n"); + cliSerial->printf("Octa frame\n"); #elif FRAME_CONFIG == HELI_FRAME - cliSerial->printf_P("Heli frame\n"); + cliSerial->printf("Heli frame\n"); #endif print_blanks(2); @@ -344,7 +344,7 @@ void Copter::report_frame() void Copter::report_radio() { - cliSerial->printf_P("Radio\n"); + cliSerial->printf("Radio\n"); print_divider(); // radio print_radio_values(); @@ -353,7 +353,7 @@ void Copter::report_radio() void Copter::report_ins() { - cliSerial->printf_P("INS\n"); + cliSerial->printf("INS\n"); print_divider(); print_gyro_offsets(); @@ -363,7 +363,7 @@ void Copter::report_ins() void Copter::report_flight_modes() { - cliSerial->printf_P("Flight modes\n"); + cliSerial->printf("Flight modes\n"); print_divider(); for(int16_t i = 0; i < 6; i++ ) { @@ -375,7 +375,7 @@ void Copter::report_flight_modes() void Copter::report_optflow() { #if OPTFLOW == ENABLED - cliSerial->printf_P("OptFlow\n"); + cliSerial->printf("OptFlow\n"); print_divider(); print_enabled(optflow.enabled()); @@ -390,32 +390,32 @@ void Copter::report_optflow() void Copter::print_radio_values() { - cliSerial->printf_P("CH1: %d | %d\n", (int)channel_roll->radio_min, (int)channel_roll->radio_max); - cliSerial->printf_P("CH2: %d | %d\n", (int)channel_pitch->radio_min, (int)channel_pitch->radio_max); - cliSerial->printf_P("CH3: %d | %d\n", (int)channel_throttle->radio_min, (int)channel_throttle->radio_max); - cliSerial->printf_P("CH4: %d | %d\n", (int)channel_yaw->radio_min, (int)channel_yaw->radio_max); - cliSerial->printf_P("CH5: %d | %d\n", (int)g.rc_5.radio_min, (int)g.rc_5.radio_max); - cliSerial->printf_P("CH6: %d | %d\n", (int)g.rc_6.radio_min, (int)g.rc_6.radio_max); - cliSerial->printf_P("CH7: %d | %d\n", (int)g.rc_7.radio_min, (int)g.rc_7.radio_max); - cliSerial->printf_P("CH8: %d | %d\n", (int)g.rc_8.radio_min, (int)g.rc_8.radio_max); + cliSerial->printf("CH1: %d | %d\n", (int)channel_roll->radio_min, (int)channel_roll->radio_max); + cliSerial->printf("CH2: %d | %d\n", (int)channel_pitch->radio_min, (int)channel_pitch->radio_max); + cliSerial->printf("CH3: %d | %d\n", (int)channel_throttle->radio_min, (int)channel_throttle->radio_max); + cliSerial->printf("CH4: %d | %d\n", (int)channel_yaw->radio_min, (int)channel_yaw->radio_max); + cliSerial->printf("CH5: %d | %d\n", (int)g.rc_5.radio_min, (int)g.rc_5.radio_max); + cliSerial->printf("CH6: %d | %d\n", (int)g.rc_6.radio_min, (int)g.rc_6.radio_max); + cliSerial->printf("CH7: %d | %d\n", (int)g.rc_7.radio_min, (int)g.rc_7.radio_max); + cliSerial->printf("CH8: %d | %d\n", (int)g.rc_8.radio_min, (int)g.rc_8.radio_max); } void Copter::print_switch(uint8_t p, uint8_t m, bool b) { - cliSerial->printf_P("Pos %d:\t",p); + cliSerial->printf("Pos %d:\t",p); print_flight_mode(cliSerial, m); - cliSerial->printf_P(",\t\tSimple: "); + cliSerial->printf(",\t\tSimple: "); if(b) - cliSerial->printf_P("ON\n"); + cliSerial->printf("ON\n"); else - cliSerial->printf_P("OFF\n"); + cliSerial->printf("OFF\n"); } void Copter::print_accel_offsets_and_scaling(void) { const Vector3f &accel_offsets = ins.get_accel_offsets(); const Vector3f &accel_scale = ins.get_accel_scale(); - cliSerial->printf_P("A_off: %4.2f, %4.2f, %4.2f\nA_scale: %4.2f, %4.2f, %4.2f\n", + cliSerial->printf("A_off: %4.2f, %4.2f, %4.2f\nA_scale: %4.2f, %4.2f, %4.2f\n", (double)accel_offsets.x, // Pitch (double)accel_offsets.y, // Roll (double)accel_offsets.z, // YAW @@ -427,7 +427,7 @@ void Copter::print_accel_offsets_and_scaling(void) void Copter::print_gyro_offsets(void) { const Vector3f &gyro_offsets = ins.get_gyro_offsets(); - cliSerial->printf_P("G_off: %4.2f, %4.2f, %4.2f\n", + cliSerial->printf("G_off: %4.2f, %4.2f, %4.2f\n", (double)gyro_offsets.x, (double)gyro_offsets.y, (double)gyro_offsets.z); @@ -438,13 +438,13 @@ void Copter::print_gyro_offsets(void) // report_compass - displays compass information. Also called by compassmot.pde void Copter::report_compass() { - cliSerial->printf_P("Compass\n"); + cliSerial->printf("Compass\n"); print_divider(); print_enabled(g.compass_enabled); // mag declination - cliSerial->printf_P("Mag Dec: %4.4f\n", + cliSerial->printf("Mag Dec: %4.4f\n", (double)degrees(compass.get_declination())); // mag offsets @@ -452,7 +452,7 @@ void Copter::report_compass() for (uint8_t i=0; iprintf_P("Mag%d off: %4.4f, %4.4f, %4.4f\n", + cliSerial->printf("Mag%d off: %4.4f, %4.4f, %4.4f\n", (int)i, (double)offsets.x, (double)offsets.y, @@ -473,7 +473,7 @@ void Copter::report_compass() Vector3f motor_compensation; for (uint8_t i=0; iprintf_P("\nComMot%d: %4.2f, %4.2f, %4.2f\n", + cliSerial->printf("\nComMot%d: %4.2f, %4.2f, %4.2f\n", (int)i, (double)motor_compensation.x, (double)motor_compensation.y, @@ -510,7 +510,7 @@ void Copter::print_enabled(bool b) void Copter::report_version() { - cliSerial->printf_P("FW Ver: %d\n",(int)g.k_format_version); + cliSerial->printf("FW Ver: %d\n",(int)g.k_format_version); print_divider(); print_blanks(2); } diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 18f178db83..222b551b6b 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -14,7 +14,7 @@ // This is the help function int8_t Copter::main_menu_help(uint8_t argc, const Menu::arg *argv) { - cliSerial->printf_P("Commands:\n" + cliSerial->printf("Commands:\n" " logs\n" " setup\n" " test\n" @@ -94,7 +94,7 @@ void Copter::init_ardupilot() // initialise serial port serial_manager.init_console(); - cliSerial->printf_P("\n\nInit " FIRMWARE_STRING + cliSerial->printf("\n\nInit " FIRMWARE_STRING "\n\nFree RAM: %u\n", hal.util->available_memory()); diff --git a/ArduCopter/test.cpp b/ArduCopter/test.cpp index 55f910b75d..81bf2ce027 100644 --- a/ArduCopter/test.cpp +++ b/ArduCopter/test.cpp @@ -46,7 +46,7 @@ int8_t Copter::test_baro(uint8_t argc, const Menu::arg *argv) if (!barometer.healthy()) { cliSerial->println("not healthy"); } else { - cliSerial->printf_P("Alt: %0.2fm, Raw: %f Temperature: %.1f\n", + cliSerial->printf("Alt: %0.2fm, Raw: %f Temperature: %.1f\n", (double)(baro_alt / 100.0f), (double)barometer.get_pressure(), (double)barometer.get_temperature()); @@ -65,7 +65,7 @@ int8_t Copter::test_compass(uint8_t argc, const Menu::arg *argv) uint8_t medium_loopCounter = 0; if (!g.compass_enabled) { - cliSerial->printf_P("Compass: "); + cliSerial->printf("Compass: "); print_enabled(false); return (0); } @@ -118,7 +118,7 @@ int8_t Copter::test_compass(uint8_t argc, const Menu::arg *argv) if (compass.healthy()) { const Vector3f &mag_ofs = compass.get_offsets(); const Vector3f &mag = compass.get_field(); - cliSerial->printf_P("Heading: %ld, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n", + cliSerial->printf("Heading: %ld, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n", (wrap_360_cd(ToDeg(heading) * 100)) /100, (double)mag.x, (double)mag.y, @@ -148,12 +148,12 @@ int8_t Copter::test_ins(uint8_t argc, const Menu::arg *argv) { Vector3f gyro, accel; print_hit_enter(); - cliSerial->printf_P("INS\n"); + cliSerial->printf("INS\n"); delay(1000); ahrs.init(); ins.init(ins_sample_rate); - cliSerial->printf_P("...done\n"); + cliSerial->printf("...done\n"); delay(50); @@ -164,7 +164,7 @@ int8_t Copter::test_ins(uint8_t argc, const Menu::arg *argv) float test = accel.length() / GRAVITY_MSS; - cliSerial->printf_P("a %7.4f %7.4f %7.4f g %7.4f %7.4f %7.4f t %7.4f \n", + cliSerial->printf("a %7.4f %7.4f %7.4f g %7.4f %7.4f %7.4f t %7.4f \n", (double)accel.x, (double)accel.y, (double)accel.z, (double)gyro.x, (double)gyro.y, (double)gyro.z, (double)test); @@ -180,14 +180,14 @@ int8_t Copter::test_optflow(uint8_t argc, const Menu::arg *argv) { #if OPTFLOW == ENABLED if(optflow.enabled()) { - cliSerial->printf_P("dev id: %d\t",(int)optflow.device_id()); + cliSerial->printf("dev id: %d\t",(int)optflow.device_id()); print_hit_enter(); while(1) { delay(200); optflow.update(); const Vector2f& flowRate = optflow.flowRate(); - cliSerial->printf_P("flowX : %7.4f\t flowY : %7.4f\t flow qual : %d\n", + cliSerial->printf("flowX : %7.4f\t flowY : %7.4f\t flow qual : %d\n", (double)flowRate.x, (double)flowRate.y, (int)optflow.quality()); @@ -197,7 +197,7 @@ int8_t Copter::test_optflow(uint8_t argc, const Menu::arg *argv) } } } else { - cliSerial->printf_P("OptFlow: "); + cliSerial->printf("OptFlow: "); print_enabled(false); } return (0); @@ -212,14 +212,14 @@ int8_t Copter::test_relay(uint8_t argc, const Menu::arg *argv) delay(1000); while(1) { - cliSerial->printf_P("Relay on\n"); + cliSerial->printf("Relay on\n"); relay.on(0); delay(3000); if(cliSerial->available() > 0) { return (0); } - cliSerial->printf_P("Relay off\n"); + cliSerial->printf("Relay off\n"); relay.off(0); delay(3000); if(cliSerial->available() > 0) { @@ -248,15 +248,15 @@ int8_t Copter::test_sonar(uint8_t argc, const Menu::arg *argv) #if CONFIG_SONAR == ENABLED sonar.init(); - cliSerial->printf_P("RangeFinder: %d devices detected\n", sonar.num_sensors()); + cliSerial->printf("RangeFinder: %d devices detected\n", sonar.num_sensors()); print_hit_enter(); while(1) { delay(100); sonar.update(); - cliSerial->printf_P("Primary: status %d distance_cm %d \n", (int)sonar.status(), sonar.distance_cm()); - cliSerial->printf_P("All: device_0 type %d status %d distance_cm %d, device_1 type %d status %d distance_cm %d\n", + cliSerial->printf("Primary: status %d distance_cm %d \n", (int)sonar.status(), sonar.distance_cm()); + cliSerial->printf("All: device_0 type %d status %d distance_cm %d, device_1 type %d status %d distance_cm %d\n", (int)sonar._type[0], (int)sonar.status(0), sonar.distance_cm(0), (int)sonar._type[1], (int)sonar.status(1), sonar.distance_cm(1)); if(cliSerial->available() > 0) { @@ -270,7 +270,7 @@ int8_t Copter::test_sonar(uint8_t argc, const Menu::arg *argv) void Copter::print_hit_enter() { - cliSerial->printf_P("Hit Enter to exit.\n\n"); + cliSerial->printf("Hit Enter to exit.\n\n"); } #endif // CLI_ENABLED diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 044c1aacdb..84fa22e6d2 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -33,7 +33,7 @@ bool Plane::print_log_menu(void) // Pass it the capitalised name of the log option, as defined // in defines.h but without the LOG_ prefix. It will check for // the bit being set and print the name of the log option to suit. - #define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf_P(" %S", # _s) + #define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf(" %S", # _s) PLOG(ATTITUDE_FAST); PLOG(ATTITUDE_MED); PLOG(GPS); @@ -71,11 +71,11 @@ int8_t Plane::dump_log(uint8_t argc, const Menu::arg *argv) DataFlash.DumpPageInfo(cliSerial); return(-1); } else if (dump_log_num <= 0) { - cliSerial->printf_P("dumping all\n"); + cliSerial->printf("dumping all\n"); Log_Read(0, 1, 0); return(-1); } else if ((argc != 2) || ((uint16_t)dump_log_num > DataFlash.get_num_logs())) { - cliSerial->printf_P("bad log number\n"); + cliSerial->printf("bad log number\n"); return(-1); } @@ -97,7 +97,7 @@ int8_t Plane::select_logs(uint8_t argc, const Menu::arg *argv) uint32_t bits; if (argc != 2) { - cliSerial->printf_P("missing log type\n"); + cliSerial->printf("missing log type\n"); return(-1); } @@ -504,7 +504,7 @@ static const struct LogStructure log_structure[] = { // Read the DataFlash.log memory : Packet Parser void Plane::Log_Read(uint16_t list_entry, int16_t start_page, int16_t end_page) { - cliSerial->printf_P("\n" FIRMWARE_STRING + cliSerial->printf("\n" FIRMWARE_STRING "\nFree RAM: %u\n", (unsigned)hal.util->available_memory()); diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 594b436224..5efd60ce08 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1259,14 +1259,14 @@ const AP_Param::ConversionInfo conversion_table[] = { void Plane::load_parameters(void) { if (!AP_Param::check_var_info()) { - cliSerial->printf_P("Bad parameter table\n"); + cliSerial->printf("Bad parameter table\n"); hal.scheduler->panic("Bad parameter table"); } if (!g.format_version.load() || g.format_version != Parameters::k_format_version) { // erase all parameters - cliSerial->printf_P("Firmware change: erasing EEPROM...\n"); + cliSerial->printf("Firmware change: erasing EEPROM...\n"); AP_Param::erase_all(); // save the current format version @@ -1277,6 +1277,6 @@ void Plane::load_parameters(void) // Load all auto-loaded EEPROM variables AP_Param::load_all(); AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table)); - cliSerial->printf_P("load_all took %luus\n", micros() - before); + cliSerial->printf("load_all took %luus\n", micros() - before); } } diff --git a/ArduPlane/setup.cpp b/ArduPlane/setup.cpp index cdf496fce8..f57ff994c8 100644 --- a/ArduPlane/setup.cpp +++ b/ArduPlane/setup.cpp @@ -19,7 +19,7 @@ MENU(setup_menu, "setup", setup_menu_commands); int8_t Plane::setup_mode(uint8_t argc, const Menu::arg *argv) { // Give the user some guidance - cliSerial->printf_P("Setup Mode\n" + cliSerial->printf("Setup Mode\n" "\n" "IMPORTANT: if you have not previously set this system up, use the\n" "'reset' command to initialize the EEPROM to sensible default values\n" @@ -37,7 +37,7 @@ int8_t Plane::setup_factory(uint8_t argc, const Menu::arg *argv) { int c; - cliSerial->printf_P("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "); + cliSerial->printf("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "); do { c = cliSerial->read(); @@ -46,7 +46,7 @@ int8_t Plane::setup_factory(uint8_t argc, const Menu::arg *argv) if (('y' != c) && ('Y' != c)) return(-1); AP_Param::erase_all(); - cliSerial->printf_P("\nFACTORY RESET complete - please reset board to continue"); + cliSerial->printf("\nFACTORY RESET complete - please reset board to continue"); for (;; ) { } @@ -59,7 +59,7 @@ int8_t Plane::setup_erase(uint8_t argc, const Menu::arg *argv) { int c; - cliSerial->printf_P("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "); + cliSerial->printf("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "); do { c = cliSerial->read(); @@ -73,9 +73,9 @@ int8_t Plane::setup_erase(uint8_t argc, const Menu::arg *argv) void Plane::zero_eeprom(void) { - cliSerial->printf_P("\nErasing EEPROM\n"); + cliSerial->printf("\nErasing EEPROM\n"); StorageManager::erase(); - cliSerial->printf_P("done\n"); + cliSerial->printf("done\n"); } #endif // CLI_ENABLED diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 8f40bd2333..1d485ba111 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -14,7 +14,7 @@ // This is the help function int8_t Plane::main_menu_help(uint8_t argc, const Menu::arg *argv) { - cliSerial->printf_P("Commands:\n" + cliSerial->printf("Commands:\n" " logs log readback/setup mode\n" " setup setup mode\n" " test test mode\n" @@ -83,7 +83,7 @@ void Plane::init_ardupilot() // initialise serial port serial_manager.init_console(); - cliSerial->printf_P("\n\nInit " FIRMWARE_STRING + cliSerial->printf("\n\nInit " FIRMWARE_STRING "\n\nFree RAM: %u\n", hal.util->available_memory()); @@ -675,7 +675,7 @@ void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) port->print("Guided"); break; default: - port->printf_P("Mode(%u)", (unsigned)mode); + port->printf("Mode(%u)", (unsigned)mode); break; } } diff --git a/ArduPlane/test.cpp b/ArduPlane/test.cpp index 6b5a0e574b..72ff85868f 100644 --- a/ArduPlane/test.cpp +++ b/ArduPlane/test.cpp @@ -40,14 +40,14 @@ MENU(test_menu, "test", test_menu_commands); int8_t Plane::test_mode(uint8_t argc, const Menu::arg *argv) { - cliSerial->printf_P("Test Mode\n\n"); + cliSerial->printf("Test Mode\n\n"); test_menu.run(); return 0; } void Plane::print_hit_enter() { - cliSerial->printf_P("Hit Enter to exit.\n\n"); + cliSerial->printf("Hit Enter to exit.\n\n"); } int8_t Plane::test_radio_pwm(uint8_t argc, const Menu::arg *argv) @@ -62,7 +62,7 @@ int8_t Plane::test_radio_pwm(uint8_t argc, const Menu::arg *argv) // ---------------------------------------------------------- read_radio(); - cliSerial->printf_P("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n", + cliSerial->printf("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n", (int)channel_roll->radio_in, (int)channel_pitch->radio_in, (int)channel_throttle->radio_in, @@ -126,7 +126,7 @@ int8_t Plane::test_radio(uint8_t argc, const Menu::arg *argv) // ------------------------------ set_servos(); - cliSerial->printf_P("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n", + cliSerial->printf("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n", (int)channel_roll->control_in, (int)channel_pitch->control_in, (int)channel_throttle->control_in, @@ -157,7 +157,7 @@ int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv) oldSwitchPosition = readSwitch(); - cliSerial->printf_P("Unplug battery, throttle in neutral, turn off radio.\n"); + cliSerial->printf("Unplug battery, throttle in neutral, turn off radio.\n"); while(channel_throttle->control_in > 0) { hal.scheduler->delay(20); read_radio(); @@ -168,19 +168,19 @@ int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv) read_radio(); if(channel_throttle->control_in > 0) { - cliSerial->printf_P("THROTTLE CHANGED %d \n", (int)channel_throttle->control_in); + cliSerial->printf("THROTTLE CHANGED %d \n", (int)channel_throttle->control_in); fail_test++; } if(oldSwitchPosition != readSwitch()) { - cliSerial->printf_P("CONTROL MODE CHANGED: "); + cliSerial->printf("CONTROL MODE CHANGED: "); print_flight_mode(cliSerial, readSwitch()); cliSerial->println(); fail_test++; } if(rc_failsafe_active()) { - cliSerial->printf_P("THROTTLE FAILSAFE ACTIVATED: %d, ", (int)channel_throttle->radio_in); + cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", (int)channel_throttle->radio_in); print_flight_mode(cliSerial, readSwitch()); cliSerial->println(); fail_test++; @@ -190,7 +190,7 @@ int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv) return (0); } if(cliSerial->available() > 0) { - cliSerial->printf_P("LOS caused no change in APM.\n"); + cliSerial->printf("LOS caused no change in APM.\n"); return (0); } } @@ -202,14 +202,14 @@ int8_t Plane::test_relay(uint8_t argc, const Menu::arg *argv) hal.scheduler->delay(1000); while(1) { - cliSerial->printf_P("Relay on\n"); + cliSerial->printf("Relay on\n"); relay.on(0); hal.scheduler->delay(3000); if(cliSerial->available() > 0) { return (0); } - cliSerial->printf_P("Relay off\n"); + cliSerial->printf("Relay off\n"); relay.off(0); hal.scheduler->delay(3000); if(cliSerial->available() > 0) { @@ -224,14 +224,14 @@ int8_t Plane::test_wp(uint8_t argc, const Menu::arg *argv) // save the alitude above home option if (g.RTL_altitude_cm < 0) { - cliSerial->printf_P("Hold current altitude\n"); + cliSerial->printf("Hold current altitude\n"); }else{ - cliSerial->printf_P("Hold altitude of %dm\n", (int)g.RTL_altitude_cm/100); + cliSerial->printf("Hold altitude of %dm\n", (int)g.RTL_altitude_cm/100); } - cliSerial->printf_P("%d waypoints\n", (int)mission.num_commands()); - cliSerial->printf_P("Hit radius: %d\n", (int)g.waypoint_radius); - cliSerial->printf_P("Loiter radius: %d\n\n", (int)g.loiter_radius); + cliSerial->printf("%d waypoints\n", (int)mission.num_commands()); + cliSerial->printf("Hit radius: %d\n", (int)g.waypoint_radius); + cliSerial->printf("Loiter radius: %d\n\n", (int)g.loiter_radius); for(uint8_t i = 0; i <= mission.num_commands(); i++) { AP_Mission::Mission_Command temp_cmd; @@ -245,7 +245,7 @@ int8_t Plane::test_wp(uint8_t argc, const Menu::arg *argv) void Plane::test_wp_print(const AP_Mission::Mission_Command& cmd) { - cliSerial->printf_P("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n", + cliSerial->printf("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n", (int)cmd.index, (int)cmd.id, (int)cmd.content.location.options, @@ -259,7 +259,7 @@ int8_t Plane::test_xbee(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); hal.scheduler->delay(1000); - cliSerial->printf_P("Begin XBee X-CTU Range and RSSI Test:\n"); + cliSerial->printf("Begin XBee X-CTU Range and RSSI Test:\n"); while(1) { @@ -278,7 +278,7 @@ int8_t Plane::test_modeswitch(uint8_t argc, const Menu::arg *argv) print_hit_enter(); hal.scheduler->delay(1000); - cliSerial->printf_P("Control CH "); + cliSerial->printf("Control CH "); cliSerial->println(FLIGHT_MODE_CHANNEL, BASE_DEC); @@ -286,7 +286,7 @@ int8_t Plane::test_modeswitch(uint8_t argc, const Menu::arg *argv) hal.scheduler->delay(20); uint8_t switchPosition = readSwitch(); if (oldSwitchPosition != switchPosition) { - cliSerial->printf_P("Position %d\n", (int)switchPosition); + cliSerial->printf("Position %d\n", (int)switchPosition); oldSwitchPosition = switchPosition; } if(cliSerial->available() > 0) { @@ -324,11 +324,11 @@ int8_t Plane::test_adc(uint8_t argc, const Menu::arg *argv) print_hit_enter(); apm1_adc.Init(); hal.scheduler->delay(1000); - cliSerial->printf_P("ADC\n"); + cliSerial->printf("ADC\n"); hal.scheduler->delay(1000); while(1) { - for (int8_t i=0; i<9; i++) cliSerial->printf_P("%.1f\t",apm1_adc.Ch(i)); + for (int8_t i=0; i<9; i++) cliSerial->printf("%.1f\t",apm1_adc.Ch(i)); cliSerial->println(); hal.scheduler->delay(100); if(cliSerial->available() > 0) { @@ -352,13 +352,13 @@ int8_t Plane::test_gps(uint8_t argc, const Menu::arg *argv) if (gps.last_message_time_ms() != last_message_time_ms) { last_message_time_ms = gps.last_message_time_ms(); const Location &loc = gps.location(); - cliSerial->printf_P("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n", + cliSerial->printf("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n", (long)loc.lat, (long)loc.lng, (long)loc.alt/100, (int)gps.num_sats()); } else { - cliSerial->printf_P("."); + cliSerial->printf("."); } if(cliSerial->available() > 0) { return (0); @@ -368,7 +368,7 @@ int8_t Plane::test_gps(uint8_t argc, const Menu::arg *argv) int8_t Plane::test_ins(uint8_t argc, const Menu::arg *argv) { - //cliSerial->printf_P("Calibrating."); + //cliSerial->printf("Calibrating."); ahrs.init(); ahrs.set_fly_forward(true); ahrs.set_wind_estimation(true); @@ -402,7 +402,7 @@ int8_t Plane::test_ins(uint8_t argc, const Menu::arg *argv) // --------------------- Vector3f gyros = ins.get_gyro(); Vector3f accels = ins.get_accel(); - cliSerial->printf_P("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n", + cliSerial->printf("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n", (int)ahrs.roll_sensor / 100, (int)ahrs.pitch_sensor / 100, (uint16_t)ahrs.yaw_sensor / 100, @@ -419,7 +419,7 @@ int8_t Plane::test_ins(uint8_t argc, const Menu::arg *argv) int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv) { if (!g.compass_enabled) { - cliSerial->printf_P("Compass: "); + cliSerial->printf("Compass: "); print_enabled(false); return (0); } @@ -465,7 +465,7 @@ int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv) if (compass.healthy()) { const Vector3f &mag_ofs = compass.get_offsets(); const Vector3f &mag = compass.get_field(); - cliSerial->printf_P("Heading: %ld, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n", + cliSerial->printf("Heading: %ld, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n", (wrap_360_cd(ToDeg(heading) * 100)) /100, (double)mag.x, (double)mag.y, (double)mag.z, (double)mag_ofs.x, (double)mag_ofs.y, (double)mag_ofs.z); @@ -493,19 +493,19 @@ int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv) int8_t Plane::test_airspeed(uint8_t argc, const Menu::arg *argv) { if (!airspeed.enabled()) { - cliSerial->printf_P("airspeed: "); + cliSerial->printf("airspeed: "); print_enabled(false); return (0); }else{ print_hit_enter(); zero_airspeed(false); - cliSerial->printf_P("airspeed: "); + cliSerial->printf("airspeed: "); print_enabled(true); while(1) { hal.scheduler->delay(20); read_airspeed(); - cliSerial->printf_P("%.1f m/s\n", (double)airspeed.get_airspeed()); + cliSerial->printf("%.1f m/s\n", (double)airspeed.get_airspeed()); if(cliSerial->available() > 0) { return (0); @@ -517,7 +517,7 @@ int8_t Plane::test_airspeed(uint8_t argc, const Menu::arg *argv) int8_t Plane::test_pressure(uint8_t argc, const Menu::arg *argv) { - cliSerial->printf_P("Uncalibrated relative airpressure\n"); + cliSerial->printf("Uncalibrated relative airpressure\n"); print_hit_enter(); init_barometer(); @@ -529,7 +529,7 @@ int8_t Plane::test_pressure(uint8_t argc, const Menu::arg *argv) if (!barometer.healthy()) { cliSerial->println("not healthy"); } else { - cliSerial->printf_P("Alt: %0.2fm, Raw: %f Temperature: %.1f\n", + cliSerial->printf("Alt: %0.2fm, Raw: %f Temperature: %.1f\n", (double)barometer.get_altitude(), (double)barometer.get_pressure(), (double)barometer.get_temperature()); @@ -544,11 +544,11 @@ int8_t Plane::test_pressure(uint8_t argc, const Menu::arg *argv) void Plane::print_enabled(bool b) { if (b) { - cliSerial->printf_P("en"); + cliSerial->printf("en"); } else { - cliSerial->printf_P("dis"); + cliSerial->printf("dis"); } - cliSerial->printf_P("abled\n"); + cliSerial->printf("abled\n"); } #endif // CLI_ENABLED diff --git a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp index bafeb3adb4..07af9c2347 100644 --- a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp +++ b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp @@ -82,7 +82,7 @@ void loop(void) if (now - last_print >= 100000 /* 100ms : 10hz */) { Vector3f drift = ahrs.get_gyro_drift(); - hal.console->printf_P( + hal.console->printf( "r:%4.1f p:%4.1f y:%4.1f " "drift=(%5.1f %5.1f %5.1f) hdg=%.1f rate=%.1f\n", ToDeg(ahrs.roll), diff --git a/libraries/AP_Common/AP_Common.h b/libraries/AP_Common/AP_Common.h index bf23f3ff00..db2d2fff3c 100644 --- a/libraries/AP_Common/AP_Common.h +++ b/libraries/AP_Common/AP_Common.h @@ -57,13 +57,13 @@ // //#pragma GCC diagnostic error "-Wfloat-equal" -// The following is strictly for type-checking arguments to printf_P calls +// The following is strictly for type-checking arguments to printf calls // in conjunction with a suitably modified Arduino IDE; never define for // production as it generates bad code. // #if defined(PRINTF_FORMAT_WARNING_DEBUG) # undef PSTR - # define PSTR(_x) _x // help the compiler with printf_P + # define PSTR(_x) _x // help the compiler with printf # define float double // silence spurious format warnings for %f #endif diff --git a/libraries/AP_Common/examples/AP_Common/AP_Common.cpp b/libraries/AP_Common/examples/AP_Common/AP_Common.cpp index c2e01d9597..5b0553a72a 100644 --- a/libraries/AP_Common/examples/AP_Common/AP_Common.cpp +++ b/libraries/AP_Common/examples/AP_Common/AP_Common.cpp @@ -17,14 +17,14 @@ void test_high_low_byte(void) for (i=0; i<=300; i++) { high = HIGHBYTE(i); low = LOWBYTE(i); - hal.console->printf_P("\ni:%u high:%u low:%u",(unsigned int)i, (unsigned int)high, (unsigned int)low); + hal.console->printf("\ni:%u high:%u low:%u",(unsigned int)i, (unsigned int)high, (unsigned int)low); } // test values from 300 to 65400 at increments of 200 for (i=301; i<=65400; i+=200) { high = HIGHBYTE(i); low = LOWBYTE(i); - hal.console->printf_P("\ni:%u high:%u low:%u",(unsigned int)i, (unsigned int)high, (unsigned int)low); + hal.console->printf("\ni:%u high:%u low:%u",(unsigned int)i, (unsigned int)high, (unsigned int)low); } } diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index d92f33f9c9..86ab88ffc3 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -276,17 +276,17 @@ AP_Compass_HMC5843::init() hal.scheduler->suspend_timer_procs(); if (!_bus_sem || !_bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - hal.console->printf_P("HMC5843: Unable to get bus semaphore\n"); + hal.console->printf("HMC5843: Unable to get bus semaphore\n"); goto fail_sem; } if (!_bus->configure()) { - hal.console->printf_P("HMC5843: Could not configure the bus\n"); + hal.console->printf("HMC5843: Could not configure the bus\n"); goto errout; } if (!_detect_version()) { - hal.console->printf_P("HMC5843: Could not detect version\n"); + hal.console->printf("HMC5843: Could not detect version\n"); goto errout; } @@ -303,7 +303,7 @@ AP_Compass_HMC5843::init() } if (!_calibrate(calibration_gain, expected_x, expected_yz)) { - hal.console->printf_P("HMC5843: Could not calibrate sensor\n"); + hal.console->printf("HMC5843: Could not calibrate sensor\n"); goto errout; } @@ -313,7 +313,7 @@ AP_Compass_HMC5843::init() } if (!_bus->start_measurements()) { - hal.console->printf_P("HMC5843: Could not start measurements on bus\n"); + hal.console->printf("HMC5843: Could not start measurements on bus\n"); goto errout; } _initialised = true; @@ -325,7 +325,7 @@ AP_Compass_HMC5843::init() read(); #if 0 - hal.console->printf_P("CalX: %.2f CalY: %.2f CalZ: %.2f\n", + hal.console->printf("CalX: %.2f CalY: %.2f CalZ: %.2f\n", _scaling[0], _scaling[1], _scaling[2]); #endif @@ -376,13 +376,13 @@ bool AP_Compass_HMC5843::_calibrate(uint8_t calibration_gain, float cal[3]; - // hal.console->printf_P("mag %d %d %d\n", _mag_x, _mag_y, _mag_z); + // hal.console->printf("mag %d %d %d\n", _mag_x, _mag_y, _mag_z); cal[0] = fabsf(expected_x / (float)_mag_x); cal[1] = fabsf(expected_yz / (float)_mag_y); cal[2] = fabsf(expected_yz / (float)_mag_z); - // hal.console->printf_P("cal=%.2f %.2f %.2f\n", cal[0], cal[1], cal[2]); + // hal.console->printf("cal=%.2f %.2f %.2f\n", cal[0], cal[1], cal[2]); // we throw away the first two samples as the compass may // still be changing its state from the application of the @@ -398,7 +398,7 @@ bool AP_Compass_HMC5843::_calibrate(uint8_t calibration_gain, if (IS_CALIBRATION_VALUE_VALID(cal[0]) && IS_CALIBRATION_VALUE_VALID(cal[1]) && IS_CALIBRATION_VALUE_VALID(cal[2])) { - // hal.console->printf_P("car=%.2f %.2f %.2f good\n", cal[0], cal[1], cal[2]); + // hal.console->printf("car=%.2f %.2f %.2f good\n", cal[0], cal[1], cal[2]); good_count++; _scaling[0] += cal[0]; @@ -410,8 +410,8 @@ bool AP_Compass_HMC5843::_calibrate(uint8_t calibration_gain, #if 0 /* useful for debugging */ - hal.console->printf_P("MagX: %d MagY: %d MagZ: %d\n", (int)_mag_x, (int)_mag_y, (int)_mag_z); - hal.console->printf_P("CalX: %.2f CalY: %.2f CalZ: %.2f\n", cal[0], cal[1], cal[2]); + hal.console->printf("MagX: %d MagY: %d MagZ: %d\n", (int)_mag_x, (int)_mag_y, (int)_mag_z); + hal.console->printf("CalX: %.2f CalY: %.2f CalZ: %.2f\n", cal[0], cal[1], cal[2]); #endif } diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 5a8c263e77..7d5642e137 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -79,7 +79,7 @@ AP_GPS_UBLOX::send_next_rate_update(void) return; } - //hal.console->printf_P("next_rate: %u\n", (unsigned)rate_update_step); + //hal.console->printf("next_rate: %u\n", (unsigned)rate_update_step); switch (rate_update_step++) { case 0: diff --git a/libraries/AP_HAL/examples/AnalogIn/AnalogIn.cpp b/libraries/AP_HAL/examples/AnalogIn/AnalogIn.cpp index d563016de9..f427a13e55 100644 --- a/libraries/AP_HAL/examples/AnalogIn/AnalogIn.cpp +++ b/libraries/AP_HAL/examples/AnalogIn/AnalogIn.cpp @@ -5,7 +5,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); AP_HAL::AnalogSource* ch; void setup (void) { - hal.console->printf_P("Starting AP_HAL::AnalogIn test\r\n"); + hal.console->printf("Starting AP_HAL::AnalogIn test\r\n"); ch = hal.analogin->channel(0); } @@ -17,7 +17,7 @@ void loop (void) if (pin == 0) { hal.console->println(); } - hal.console->printf_P("[%u %.3f] ", + hal.console->printf("[%u %.3f] ", (unsigned)pin, v); pin = (pin+1) % 16; ch->set_pin(pin); diff --git a/libraries/AP_HAL/examples/Storage/Storage.cpp b/libraries/AP_HAL/examples/Storage/Storage.cpp index 2af5a1e150..addc0f6436 100644 --- a/libraries/AP_HAL/examples/Storage/Storage.cpp +++ b/libraries/AP_HAL/examples/Storage/Storage.cpp @@ -13,7 +13,7 @@ void setup(void) /* init Storage API */ - hal.console->printf_P("Starting AP_HAL::Storage test\r\n"); + hal.console->printf("Starting AP_HAL::Storage test\r\n"); st->init(NULL); /* @@ -33,7 +33,7 @@ void setup(void) /* print XORed result */ - hal.console->printf_P("XORed ememory: %u\r\n", (unsigned) XOR_res); + hal.console->printf("XORed ememory: %u\r\n", (unsigned) XOR_res); } // In main loop do nothing diff --git a/libraries/AP_HAL_Empty/Scheduler.cpp b/libraries/AP_HAL_Empty/Scheduler.cpp index b18e5a7c11..c2f85df8ee 100644 --- a/libraries/AP_HAL_Empty/Scheduler.cpp +++ b/libraries/AP_HAL_Empty/Scheduler.cpp @@ -78,7 +78,7 @@ void EmptyScheduler::panic(const prog_char_t *errormsg, ...) va_start(ap, errormsg); hal.console->vprintf(errormsg, ap); va_end(ap); - hal.console->printf_P("\n"); + hal.console->printf("\n"); for(;;); } diff --git a/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp b/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp index 5780ed19e1..2dccda9d1b 100644 --- a/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp +++ b/libraries/AP_HAL_FLYMAPLE/Scheduler.cpp @@ -241,7 +241,7 @@ void FLYMAPLEScheduler::panic(const prog_char_t *errormsg, ...) { va_start(ap, errormsg); hal.console->vprintf(errormsg, ap); va_end(ap); - hal.console->printf_P("\n"); + hal.console->printf("\n"); for(;;); } diff --git a/libraries/AP_HAL_FLYMAPLE/examples/Console/Console.cpp b/libraries/AP_HAL_FLYMAPLE/examples/Console/Console.cpp index cee909b4fe..b411d66dbf 100644 --- a/libraries/AP_HAL_FLYMAPLE/examples/Console/Console.cpp +++ b/libraries/AP_HAL_FLYMAPLE/examples/Console/Console.cpp @@ -31,7 +31,7 @@ void setup(void) hal.console->println(1000, 16); hal.console->println("progmem"); hal.console->printf("printf %d %u %#x %p %f %S\n", -1000, 1000, 1000, 1000, 1.2345, "progmem"); - hal.console->printf_P("printf_P %d %u %#x %p %f %S\n", -1000, 1000, 1000, 1000, 1.2345, "progmem"); + hal.console->printf("printf %d %u %#x %p %f %S\n", -1000, 1000, 1000, 1000, 1.2345, "progmem"); hal.console->println("done."); for(;;); diff --git a/libraries/AP_HAL_FLYMAPLE/examples/I2CDriver_HMC5883L/I2CDriver_HMC5883L.cpp b/libraries/AP_HAL_FLYMAPLE/examples/I2CDriver_HMC5883L/I2CDriver_HMC5883L.cpp index 51ddb60138..3fa884e75c 100644 --- a/libraries/AP_HAL_FLYMAPLE/examples/I2CDriver_HMC5883L/I2CDriver_HMC5883L.cpp +++ b/libraries/AP_HAL_FLYMAPLE/examples/I2CDriver_HMC5883L/I2CDriver_HMC5883L.cpp @@ -17,14 +17,14 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); #define HMC5883L 0x1E void setup() { - hal.console->printf_P("Initializing HMC5883L at address %x\r\n", + hal.console->printf("Initializing HMC5883L at address %x\r\n", HMC5883L); uint8_t stat = hal.i2c->writeRegister(HMC5883L,0x02,0x00); if (stat == 0) { - hal.console->printf_P("successful init\r\n"); + hal.console->printf("successful init\r\n"); } else { - hal.console->printf_P("failed init: return status %d\r\n", + hal.console->printf("failed init: return status %d\r\n", (int)stat); for(;;); } @@ -43,9 +43,9 @@ void loop() { y |= data[3]; z = data[4] << 8; z |= data[5]; - hal.console->printf_P("x: %d y: %d z: %d \r\n", x, y, z); + hal.console->printf("x: %d y: %d z: %d \r\n", x, y, z); } else { - hal.console->printf_P("i2c error: status %d\r\n", (int)stat); + hal.console->printf("i2c error: status %d\r\n", (int)stat); } } diff --git a/libraries/AP_HAL_FLYMAPLE/examples/RCInput/RCInput.cpp b/libraries/AP_HAL_FLYMAPLE/examples/RCInput/RCInput.cpp index 32b823af90..9b18b0de6d 100644 --- a/libraries/AP_HAL_FLYMAPLE/examples/RCInput/RCInput.cpp +++ b/libraries/AP_HAL_FLYMAPLE/examples/RCInput/RCInput.cpp @@ -14,7 +14,7 @@ void multiread(AP_HAL::RCInput* in, uint16_t* channels) { bool valid; valid = in->new_input(); in->read(channels, 8); - hal.console->printf_P( + hal.console->printf( "multi read %d: %d %d %d %d %d %d %d %d\r\n", (int) valid, channels[0], channels[1], channels[2], channels[3], @@ -28,7 +28,7 @@ void individualread(AP_HAL::RCInput* in, uint16_t* channels) { for (int i = 0; i < 8; i++) { channels[i] = in->read(i); } - hal.console->printf_P( + hal.console->printf( "individual read %d: %d %d %d %d %d %d %d %d\r\n", (int) valid, channels[0], channels[1], channels[2], channels[3], diff --git a/libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/RCPassthroughTest.cpp b/libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/RCPassthroughTest.cpp index 644201c7d9..3755cb104a 100644 --- a/libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/RCPassthroughTest.cpp +++ b/libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/RCPassthroughTest.cpp @@ -13,7 +13,7 @@ void multiread(AP_HAL::RCInput* in, uint16_t* channels) { /* Multi-channel read method: */ uint8_t valid; valid = in->read(channels, 8); - hal.console->printf_P( + hal.console->printf( "multi read %d: %d %d %d %d %d %d %d %d\r\n", (int) valid, channels[0], channels[1], channels[2], channels[3], @@ -27,7 +27,7 @@ void individualread(AP_HAL::RCInput* in, uint16_t* channels) { for (int i = 0; i < 8; i++) { channels[i] = in->read(i); } - hal.console->printf_P( + hal.console->printf( "individual read %d: %d %d %d %d %d %d %d %d\r\n", (int) valid, channels[0], channels[1], channels[2], channels[3], @@ -76,7 +76,7 @@ void setup (void) { /* Bottom 4 channels at 400hz (like on a quad) */ hal.rcout->set_freq(0x0000000F, 400); for(int i = 0; i < 12; i++) { - hal.console->printf_P("rcout ch %d has frequency %d\r\n", + hal.console->printf("rcout ch %d has frequency %d\r\n", i, hal.rcout->get_freq(i)); } /* Delay to let the user see the above printouts on the terminal */ diff --git a/libraries/AP_HAL_FLYMAPLE/examples/SPIDriver/SPIDriver.cpp b/libraries/AP_HAL_FLYMAPLE/examples/SPIDriver/SPIDriver.cpp index 120f0ffbac..b67a97c32f 100644 --- a/libraries/AP_HAL_FLYMAPLE/examples/SPIDriver/SPIDriver.cpp +++ b/libraries/AP_HAL_FLYMAPLE/examples/SPIDriver/SPIDriver.cpp @@ -16,7 +16,7 @@ AP_HAL::SPIDeviceDriver* spidev; void setup (void) { hal.scheduler->delay(5000); - hal.console->printf_P("Starting AP_HAL_FLYMAPLE::SPIDriver test\r\n"); + hal.console->printf("Starting AP_HAL_FLYMAPLE::SPIDriver test\r\n"); spidev = hal.spi->device(AP_HAL::SPIDevice_MPU6000); // Not really MPU6000, just a generic SPU driver if (!spidev) diff --git a/libraries/AP_HAL_FLYMAPLE/examples/Scheduler/Scheduler.cpp b/libraries/AP_HAL_FLYMAPLE/examples/Scheduler/Scheduler.cpp index f9e6cad79c..49f9cfd66b 100644 --- a/libraries/AP_HAL_FLYMAPLE/examples/Scheduler/Scheduler.cpp +++ b/libraries/AP_HAL_FLYMAPLE/examples/Scheduler/Scheduler.cpp @@ -53,7 +53,7 @@ void schedule_toggle_hang(void) { } void setup_pin(int pin_num) { - hal.console->printf_P("Setup pin %d\r\n", pin_num); + hal.console->printf("Setup pin %d\r\n", pin_num); hal.gpio->pinMode(pin_num,HAL_GPIO_OUTPUT); /* Blink so we can see setup on the logic analyzer.*/ hal.gpio->write(pin_num,1); @@ -62,21 +62,21 @@ void setup_pin(int pin_num) { void setup (void) { // hal.scheduler->delay(5000); - hal.console->printf_P("Starting AP_HAL_AVR::Scheduler test\r\n"); + hal.console->printf("Starting AP_HAL_AVR::Scheduler test\r\n"); setup_pin(DELAY_TOGGLE_PIN); setup_pin(FAILSAFE_TOGGLE_PIN); setup_pin(SCHEDULED_TOGGLE_PIN_1); setup_pin(SCHEDULED_TOGGLE_PIN_2); - hal.console->printf_P("Testing delay callback. " + hal.console->printf("Testing delay callback. " "Pin %d should toggle at 1khz:\r\n", (int) DELAY_TOGGLE_PIN); // hal.scheduler->register_delay_callback(delay_toggle,0); hal.scheduler->delay(2000); - hal.console->printf_P("Testing failsafe callback. " + hal.console->printf("Testing failsafe callback. " "Pin %d should toggle at 1khz:\r\n", (int) FAILSAFE_TOGGLE_PIN); @@ -84,10 +84,10 @@ void setup (void) { hal.scheduler->delay(2000); - hal.console->printf_P("Testing running timer processes.\r\n"); - hal.console->printf_P("Pin %d should toggle at 1khz.\r\n", + hal.console->printf("Testing running timer processes.\r\n"); + hal.console->printf("Pin %d should toggle at 1khz.\r\n", (int) SCHEDULED_TOGGLE_PIN_1); - hal.console->printf_P("Pin %d should toggle at 1khz.\r\n", + hal.console->printf("Pin %d should toggle at 1khz.\r\n", (int) SCHEDULED_TOGGLE_PIN_2); hal.scheduler->register_timer_process(schedule_toggle_1); @@ -97,10 +97,10 @@ void setup (void) { // not yet working on flymaple: see FLYMAPLEScheduler::_timer_procs_timer_event() #if 1 - hal.console->printf_P("Test running a pathological timer process.\r\n" + hal.console->printf("Test running a pathological timer process.\r\n" "Failsafe should continue even as pathological process " "dominates the processor."); - hal.console->printf_P("Pin %d should toggle then go high forever.\r\n", + hal.console->printf("Pin %d should toggle then go high forever.\r\n", (int) SCHEDULED_TOGGLE_PIN_2); hal.scheduler->delay(200); hal.scheduler->register_timer_process(schedule_toggle_hang); diff --git a/libraries/AP_HAL_FLYMAPLE/examples/Semaphore/Semaphore.cpp b/libraries/AP_HAL_FLYMAPLE/examples/Semaphore/Semaphore.cpp index 8d092e1d52..2f58fc4326 100644 --- a/libraries/AP_HAL_FLYMAPLE/examples/Semaphore/Semaphore.cpp +++ b/libraries/AP_HAL_FLYMAPLE/examples/Semaphore/Semaphore.cpp @@ -53,7 +53,7 @@ void blink_a3() { } void setup_pin(int pin_num) { - hal.console->printf_P("Setup pin %d\r\n", pin_num); + hal.console->printf("Setup pin %d\r\n", pin_num); hal.gpio->pinMode(pin_num,HAL_GPIO_OUTPUT); /* Blink so we can see setup on the logic analyzer.*/ hal.gpio->write(pin_num,1); @@ -61,14 +61,14 @@ void setup_pin(int pin_num) { } void setup (void) { - hal.console->printf_P("Starting Semaphore test\r\n"); + hal.console->printf("Starting Semaphore test\r\n"); setup_pin(PIN_A0); setup_pin(PIN_A1); setup_pin(PIN_A2); setup_pin(PIN_A3); - hal.console->printf_P("Using SPIDeviceManager builtin Semaphore\r\n"); + hal.console->printf("Using SPIDeviceManager builtin Semaphore\r\n"); AP_HAL::SPIDeviceDriver *dataflash = hal.spi->device(AP_HAL::SPIDevice_Dataflash); // not really diff --git a/libraries/AP_HAL_FLYMAPLE/examples/Storage/Storage.cpp b/libraries/AP_HAL_FLYMAPLE/examples/Storage/Storage.cpp index f999e2f4ba..2d19600b22 100644 --- a/libraries/AP_HAL_FLYMAPLE/examples/Storage/Storage.cpp +++ b/libraries/AP_HAL_FLYMAPLE/examples/Storage/Storage.cpp @@ -12,40 +12,40 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); uint8_t fibs[] = { 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 0 }; void test_erase() { - hal.console->printf_P("erasing... "); + hal.console->printf("erasing... "); for(int i = 0; i < 100; i++) { hal.storage->write_byte(i, 0); } - hal.console->printf_P(" done.\r\n"); + hal.console->printf(" done.\r\n"); } void test_write() { - hal.console->printf_P("writing... "); + hal.console->printf("writing... "); hal.storage->write_block(0, fibs, sizeof(fibs)); - hal.console->printf_P(" done.\r\n"); + hal.console->printf(" done.\r\n"); } void test_readback() { - hal.console->printf_P("reading back...\r\n"); + hal.console->printf("reading back...\r\n"); uint8_t readback[sizeof(fibs)]; bool success = true; hal.storage->read_block(readback, 0, sizeof(fibs)); for (int i = 0; i < sizeof(fibs); i++) { if (readback[i] != fibs[i]) { success = false; - hal.console->printf_P("At index %d expected %d got %d\r\n", + hal.console->printf("At index %d expected %d got %d\r\n", i, (int) fibs[i], (int) readback[i]); } } if (success) { - hal.console->printf_P("all bytes read successfully\r\n"); + hal.console->printf("all bytes read successfully\r\n"); } - hal.console->printf_P("done reading back.\r\n"); + hal.console->printf("done reading back.\r\n"); } void setup (void) { hal.scheduler->delay(5000); - hal.console->printf_P("Starting AP_HAL_FLYMAPLE::Storage test\r\n"); + hal.console->printf("Starting AP_HAL_FLYMAPLE::Storage test\r\n"); hal.console->printf("test %d\n", i); test_readback(); // Test what was left from the last run, possibly after power off diff --git a/libraries/AP_HAL_FLYMAPLE/examples/UARTDriver/UARTDriver.cpp b/libraries/AP_HAL_FLYMAPLE/examples/UARTDriver/UARTDriver.cpp index 9955d24523..25a3273926 100644 --- a/libraries/AP_HAL_FLYMAPLE/examples/UARTDriver/UARTDriver.cpp +++ b/libraries/AP_HAL_FLYMAPLE/examples/UARTDriver/UARTDriver.cpp @@ -34,7 +34,7 @@ void setup(void) int x = 3; int *ptr = &x; hal.uartA->printf("printf %d %u %#x %p %f %s\n", -1000, 1000, 1000, ptr, 1.2345, "progmem"); - hal.uartA->printf_P("printf_P %d %u %#x %p %f %s\n", -1000, 1000, 1000, ptr, 1.2345, "progmem"); + hal.uartA->printf("printf %d %u %#x %p %f %s\n", -1000, 1000, 1000, ptr, 1.2345, "progmem"); hal.uartA->println("done"); } diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index ca89c5fa91..66c562f3d7 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -266,7 +266,7 @@ void SITLScheduler::panic(const prog_char_t *errormsg, ...) va_start(ap, errormsg); hal.console->vprintf(errormsg, ap); va_end(ap); - hal.console->printf_P("\n"); + hal.console->printf("\n"); for(;;); } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 36dd27e693..52b546bc37 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -541,7 +541,7 @@ bool AP_InertialSensor::_calculate_trim(const Vector3f &accel_sample, float& tri hal.console->println("trim over maximum of 10 degrees"); return false; } - hal.console->printf_P("Trim OK: roll=%.2f pitch=%.2f\n", + hal.console->printf("Trim OK: roll=%.2f pitch=%.2f\n", (double)degrees(trim_roll), (double)degrees(trim_pitch)); return true; @@ -1017,7 +1017,7 @@ AP_InertialSensor::_init_gyro() hal.console->println(); for (uint8_t k=0; kprintf_P("gyro[%u] did not converge: diff=%f dps\n", + hal.console->printf("gyro[%u] did not converge: diff=%f dps\n", (unsigned)k, (double)ToDeg(best_diff[k])); _gyro_offset[k] = best_avg[k]; // flag calibration as failed for this gyro diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index 288fbc0fe4..b4e190ae58 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -809,7 +809,7 @@ void AP_InertialSensor_LSM9DS0::_dump_registers(void) const uint8_t last = ACT_DUR; for (uint8_t reg=first; reg<=last; reg++) { uint8_t v = _register_read_g(reg); - hal.console->printf_P("%02x:%02x ", (unsigned)reg, (unsigned)v); + hal.console->printf("%02x:%02x ", (unsigned)reg, (unsigned)v); if ((reg - (first-1)) % 16 == 0) { hal.console->println(); } @@ -819,7 +819,7 @@ void AP_InertialSensor_LSM9DS0::_dump_registers(void) hal.console->println("Accelerometer and Magnetometers registers:"); for (uint8_t reg=first; reg<=last; reg++) { uint8_t v = _register_read_xm(reg); - hal.console->printf_P("%02x:%02x ", (unsigned)reg, (unsigned)v); + hal.console->printf("%02x:%02x ", (unsigned)reg, (unsigned)v); if ((reg - (first-1)) % 16 == 0) { hal.console->println(); } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index be6a751402..eee4cff5a0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -370,7 +370,7 @@ void AP_MPU6000_BusDriver_I2C::read_data_transaction(uint8_t *samples, ret = _i2c->readRegisters(_addr, MPUREG_FIFO_COUNTH, 2, _rx); if(ret != 0) { - hal.console->printf_P("MPU6000: error in i2c read\n"); + hal.console->printf("MPU6000: error in i2c read\n"); n_samples = 0; return; } @@ -380,7 +380,7 @@ void AP_MPU6000_BusDriver_I2C::read_data_transaction(uint8_t *samples, n_samples = bytes_read / MPU6000_SAMPLE_SIZE; if(n_samples > 3) { - hal.console->printf_P("bytes_read = %d, n_samples %d > 3, dropping samples\n", + hal.console->printf("bytes_read = %d, n_samples %d > 3, dropping samples\n", bytes_read, n_samples); /* Too many samples, do a FIFO RESET */ @@ -399,7 +399,7 @@ void AP_MPU6000_BusDriver_I2C::read_data_transaction(uint8_t *samples, } if(ret != 0) { - hal.console->printf_P("MPU6000: error in i2c read %d bytes\n", + hal.console->printf("MPU6000: error in i2c read %d bytes\n", n_samples * MPU6000_SAMPLE_SIZE); n_samples = 0; return; @@ -767,10 +767,10 @@ void AP_InertialSensor_MPU6000::_register_write_check(uint8_t reg, uint8_t val) _register_write(reg, val); readed = _register_read(reg); if (readed != val){ - hal.console->printf_P("Values doesn't match; written: %02x; read: %02x ", val, readed); + hal.console->printf("Values doesn't match; written: %02x; read: %02x ", val, readed); } #if MPU6000_DEBUG - hal.console->printf_P("Values written: %02x; readed: %02x ", val, readed); + hal.console->printf("Values written: %02x; readed: %02x ", val, readed); #endif } @@ -871,7 +871,7 @@ void AP_InertialSensor_MPU6000::_dump_registers(void) if (_bus_sem->take(100)) { for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) { uint8_t v = _register_read(reg); - hal.console->printf_P("%02x:%02x ", (unsigned)reg, (unsigned)v); + hal.console->printf("%02x:%02x ", (unsigned)reg, (unsigned)v); if ((reg - (MPUREG_PRODUCT_ID-1)) % 16 == 0) { hal.console->println(); } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp index fe2101952c..b807c2403f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp @@ -974,7 +974,7 @@ int16_t AP_InertialSensor_MPU9150::mpu_read_fifo(int16_t *gyro, int16_t *accel, if (fifo_count < packet_size){ return 0; } - // hal.console->printf_P(PTR("FIFO count: %hd\n", fifo_count)); + // hal.console->printf(PTR("FIFO count: %hd\n", fifo_count)); if (fifo_count > (st.hw->max_fifo >> 1)) { /* FIFO is 50% full, better check overflow bit. */ hal.i2c->readRegister(st.hw->addr, st.reg->int_status, data); @@ -1046,7 +1046,7 @@ void AP_InertialSensor_MPU9150::_accumulate(void) return; } - // hal.console->printf_P(PTR("FIFO count: %hd\n", fifo_count)); + // hal.console->printf(PTR("FIFO count: %hd\n", fifo_count)); if (fifo_count > (st.hw->max_fifo >> 1)) { /* FIFO is 50% full, better check overflow bit. */ hal.i2c->readRegister(st.hw->addr, st.reg->int_status, data); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index 57506e9330..0cc6e3495e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -531,7 +531,7 @@ void AP_InertialSensor_MPU9250::_dump_registers(AP_HAL::SPIDeviceDriver *spi) hal.console->println("MPU9250 registers"); for (uint8_t reg=0; reg<=126; reg++) { uint8_t v = _register_read(spi, reg); - hal.console->printf_P("%02x:%02x ", (unsigned)reg, (unsigned)v); + hal.console->printf("%02x:%02x ", (unsigned)reg, (unsigned)v); if ((reg - (MPUREG_PRODUCT_ID-1)) % 16 == 0) { hal.console->println(); } diff --git a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.cpp b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.cpp index e7ceb29d8a..e931ca65ff 100644 --- a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.cpp +++ b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.cpp @@ -99,17 +99,17 @@ static void display_offsets_and_scaling() Vector3f gyro_offsets = ins.get_gyro_offsets(); // display results - hal.console->printf_P( + hal.console->printf( "\nAccel Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n", accel_offsets.x, accel_offsets.y, accel_offsets.z); - hal.console->printf_P( + hal.console->printf( "Accel Scale X:%10.8f \t Y:%10.8f \t Z:%10.8f\n", accel_scale.x, accel_scale.y, accel_scale.z); - hal.console->printf_P( + hal.console->printf( "Gyro Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n", gyro_offsets.x, gyro_offsets.y, @@ -146,7 +146,7 @@ static void run_test() if (counter++ % 50 == 0) { // display results - hal.console->printf_P("Accel X:%4.2f \t Y:%4.2f \t Z:%4.2f \t len:%4.2f \t Gyro X:%4.2f \t Y:%4.2f \t Z:%4.2f\n", + hal.console->printf("Accel X:%4.2f \t Y:%4.2f \t Z:%4.2f \t len:%4.2f \t Gyro X:%4.2f \t Y:%4.2f \t Z:%4.2f\n", accel.x, accel.y, accel.z, length, gyro.x, gyro.y, gyro.z); } } diff --git a/libraries/AP_Math/examples/eulers/eulers.cpp b/libraries/AP_Math/examples/eulers/eulers.cpp index fe3eb5ecc1..b0fe97f663 100644 --- a/libraries/AP_Math/examples/eulers/eulers.cpp +++ b/libraries/AP_Math/examples/eulers/eulers.cpp @@ -49,7 +49,7 @@ static void check_result(const char *msg, ToDeg(rad_diff(pitch, -PI/2)) < 1) { // we expect breakdown at these poles #if SHOW_POLES_BREAKDOWN - hal.console->printf_P( + hal.console->printf( "%s breakdown eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n", msg, ToDeg(roll), ToDeg(roll2), @@ -57,7 +57,7 @@ static void check_result(const char *msg, ToDeg(yaw), ToDeg(yaw2)); #endif } else { - hal.console->printf_P( + hal.console->printf( "%s incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n", msg, ToDeg(roll), ToDeg(roll2), diff --git a/libraries/AP_Math/examples/polygon/polygon.cpp b/libraries/AP_Math/examples/polygon/polygon.cpp index c24ef06b37..a66d6005ac 100644 --- a/libraries/AP_Math/examples/polygon/polygon.cpp +++ b/libraries/AP_Math/examples/polygon/polygon.cpp @@ -80,7 +80,7 @@ void setup(void) bool result; result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_SIZE(OBC_boundary)); - hal.console->printf_P("%10f,%10f %s %s\n", + hal.console->printf("%10f,%10f %s %s\n", 1.0e-7f*test_points[i].point.x, 1.0e-7f*test_points[i].point.y, result ? "OUTSIDE" : "INSIDE ", diff --git a/libraries/AP_Math/examples/rotations/rotations.cpp b/libraries/AP_Math/examples/rotations/rotations.cpp index c466cddc2c..d164405b7d 100644 --- a/libraries/AP_Math/examples/rotations/rotations.cpp +++ b/libraries/AP_Math/examples/rotations/rotations.cpp @@ -41,7 +41,7 @@ static void test_rotation_accuracy(void) attitude.to_euler(&roll, &pitch, &yaw); // display results - hal.console->printf_P( + hal.console->printf( "actual angle: %d\tcalculated angle:%4.2f\n", (int)i,ToDeg(yaw)); } diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp index 81d980040c..fa8aa9d1c5 100644 --- a/libraries/AP_Math/location.cpp +++ b/libraries/AP_Math/location.cpp @@ -242,9 +242,9 @@ void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon) // print output including the minus sign if( lat_or_lon < 0 ) { - s->printf_P("-"); + s->printf("-"); } - s->printf_P("%ld.%07ld",(long)dec_portion,(long)frac_portion); + s->printf("%ld.%07ld",(long)dec_portion,(long)frac_portion); } #if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 diff --git a/libraries/AP_Menu/AP_Menu.cpp b/libraries/AP_Menu/AP_Menu.cpp index f67b15c819..687c199d20 100644 --- a/libraries/AP_Menu/AP_Menu.cpp +++ b/libraries/AP_Menu/AP_Menu.cpp @@ -84,7 +84,7 @@ Menu::_check_for_input(void) void Menu::_display_prompt(void) { - _port->printf_P("%S] ", _prompt); + _port->printf("%S] ", _prompt); } // run the menu @@ -231,7 +231,7 @@ Menu::_help(void) _port->println("Commands:"); for (i = 0; i < _entries; i++) { hal.scheduler->delay(10); - _port->printf_P(" %S\n", _commands[i].command); + _port->printf(" %S\n", _commands[i].command); } } diff --git a/libraries/AP_Mission/examples/AP_Mission_test/AP_Mission_test.cpp b/libraries/AP_Mission/examples/AP_Mission_test/AP_Mission_test.cpp index 6dd7d4154b..a2ce511983 100644 --- a/libraries/AP_Mission/examples/AP_Mission_test/AP_Mission_test.cpp +++ b/libraries/AP_Mission/examples/AP_Mission_test/AP_Mission_test.cpp @@ -59,10 +59,10 @@ bool MissionTest::start_cmd(const AP_Mission::Mission_Command& cmd) // reset tracking of number of iterations of this command (we simulate all nav commands taking 3 iterations to complete, all do command 1 iteration) if (AP_Mission::is_nav_cmd(cmd)) { num_nav_cmd_runs = 0; - hal.console->printf_P("started cmd #%d id:%d Nav\n",(int)cmd.index,(int)cmd.id); + hal.console->printf("started cmd #%d id:%d Nav\n",(int)cmd.index,(int)cmd.id); }else{ num_do_cmd_runs = 0; - hal.console->printf_P("started cmd #%d id:%d Do\n",(int)cmd.index,(int)cmd.id); + hal.console->printf("started cmd #%d id:%d Do\n",(int)cmd.index,(int)cmd.id); } return true; @@ -75,19 +75,19 @@ bool MissionTest::verify_cmd(const AP_Mission::Mission_Command& cmd) if (AP_Mission::is_nav_cmd(cmd)) { num_nav_cmd_runs++; if (num_nav_cmd_runs < verify_nav_cmd_iterations_to_complete) { - hal.console->printf_P("verified cmd #%d id:%d Nav iteration:%d\n",(int)cmd.index,(int)cmd.id,(int)num_nav_cmd_runs); + hal.console->printf("verified cmd #%d id:%d Nav iteration:%d\n",(int)cmd.index,(int)cmd.id,(int)num_nav_cmd_runs); return false; }else{ - hal.console->printf_P("verified cmd #%d id:%d Nav complete!\n",(int)cmd.index,(int)cmd.id); + hal.console->printf("verified cmd #%d id:%d Nav complete!\n",(int)cmd.index,(int)cmd.id); return true; } }else{ num_do_cmd_runs++; if (num_do_cmd_runs < verify_do_cmd_iterations_to_complete) { - hal.console->printf_P("verified cmd #%d id:%d Do iteration:%d\n",(int)cmd.index,(int)cmd.id,(int)num_do_cmd_runs); + hal.console->printf("verified cmd #%d id:%d Do iteration:%d\n",(int)cmd.index,(int)cmd.id,(int)num_do_cmd_runs); return false; }else{ - hal.console->printf_P("verified cmd #%d id:%d Do complete!\n",(int)cmd.index,(int)cmd.id); + hal.console->printf("verified cmd #%d id:%d Do complete!\n",(int)cmd.index,(int)cmd.id); return true; } } @@ -645,11 +645,11 @@ void MissionTest::print_mission() // check for empty mission if (mission.num_commands() == 0) { - hal.console->printf_P("No Mission!\n"); + hal.console->printf("No Mission!\n"); return; } - hal.console->printf_P("Mission: %d commands\n",(int)mission.num_commands()); + hal.console->printf("Mission: %d commands\n",(int)mission.num_commands()); // print each command for(uint16_t i=0; iprintf_P("Cmd#%d mav-id:%d ", (int)cmd.index, (int)cmd.id); + hal.console->printf("Cmd#%d mav-id:%d ", (int)cmd.index, (int)cmd.id); // print whether nav or do command if (AP_Mission::is_nav_cmd(cmd)) { - hal.console->printf_P("Nav "); + hal.console->printf("Nav "); }else{ - hal.console->printf_P("Do "); + hal.console->printf("Do "); } // print command contents if (cmd.id == MAV_CMD_DO_JUMP) { - hal.console->printf_P("jump-to:%d num_times:%d\n", (int)cmd.content.jump.target, (int)cmd.content.jump.num_times); + hal.console->printf("jump-to:%d num_times:%d\n", (int)cmd.content.jump.target, (int)cmd.content.jump.num_times); }else{ - hal.console->printf_P("p1:%d lat:%ld lng:%ld alt:%ld\n",(int)cmd.p1, (long)cmd.content.location.lat, (long)cmd.content.location.lng, (long)cmd.content.location.alt); + hal.console->printf("p1:%d lat:%ld lng:%ld alt:%ld\n",(int)cmd.p1, (long)cmd.content.location.lat, (long)cmd.content.location.lng, (long)cmd.content.location.alt); } } - hal.console->printf_P("--------\n"); + hal.console->printf("--------\n"); } // run_resume_test - tests the stop and resume feature @@ -752,7 +752,7 @@ void MissionTest::run_resume_test() print_mission(); // start mission - hal.console->printf_P("\nRunning missions\n"); + hal.console->printf("\nRunning missions\n"); mission.start(); // update the mission for X iterations @@ -764,7 +764,7 @@ void MissionTest::run_resume_test() } // simulate user pausing the mission - hal.console->printf_P("Stopping mission\n"); + hal.console->printf("Stopping mission\n"); mission.stop(); // update the mission for 5 seconds (nothing should happen) @@ -774,7 +774,7 @@ void MissionTest::run_resume_test() } // simulate user resuming mission - hal.console->printf_P("Resume mission\n"); + hal.console->printf("Resume mission\n"); mission.resume(); // update the mission forever @@ -869,7 +869,7 @@ void MissionTest::run_set_current_cmd_test() print_mission(); // start mission - hal.console->printf_P("\nRunning missions\n"); + hal.console->printf("\nRunning missions\n"); mission.start(); // update the mission for X iterations to let it go to about command 3 or 4 @@ -878,7 +878,7 @@ void MissionTest::run_set_current_cmd_test() } // simulate user setting current command to 2 - hal.console->printf_P("Setting current command to 2\n"); + hal.console->printf("Setting current command to 2\n"); mission.set_current_cmd(2); // update the mission forever @@ -974,7 +974,7 @@ void MissionTest::run_set_current_cmd_while_stopped_test() print_mission(); // start mission - hal.console->printf_P("\nRunning missions\n"); + hal.console->printf("\nRunning missions\n"); mission.start(); // update the mission for X iterations @@ -983,11 +983,11 @@ void MissionTest::run_set_current_cmd_while_stopped_test() } // simulate user pausing the mission - hal.console->printf_P("Stopping mission\n"); + hal.console->printf("Stopping mission\n"); mission.stop(); // simulate user setting current command to 2 - hal.console->printf_P("Setting current command to 2\n"); + hal.console->printf("Setting current command to 2\n"); mission.set_current_cmd(2); // update the mission for 2 seconds (nothing should happen) @@ -997,7 +997,7 @@ void MissionTest::run_set_current_cmd_while_stopped_test() } // simulate user resuming mission - hal.console->printf_P("Resume mission\n"); + hal.console->printf("Resume mission\n"); mission.resume(); // wait for the mission to complete @@ -1012,11 +1012,11 @@ void MissionTest::run_set_current_cmd_while_stopped_test() } // simulate user setting current command to 2 now that the mission has completed - hal.console->printf_P("Setting current command to 5\n"); + hal.console->printf("Setting current command to 5\n"); mission.set_current_cmd(5); // simulate user resuming mission - hal.console->printf_P("Resume mission\n"); + hal.console->printf("Resume mission\n"); mission.resume(); // keep running the mission forever @@ -1100,7 +1100,7 @@ void MissionTest::run_replace_cmd_test() print_mission(); // start mission - hal.console->printf_P("\nRunning missions\n"); + hal.console->printf("\nRunning missions\n"); mission.start(); // update the mission for X iterations to let it go to about command 3 or 4 @@ -1117,9 +1117,9 @@ void MissionTest::run_replace_cmd_test() cmd.content.location.lat = 12345678; cmd.content.location.lng = 23456789; if (!mission.replace_cmd(4, cmd)) { - hal.console->printf_P("failed to replace command 4\n"); + hal.console->printf("failed to replace command 4\n"); }else{ - hal.console->printf_P("replaced command #4 -------------\n"); + hal.console->printf("replaced command #4 -------------\n"); // print current mission print_mission(); } @@ -1150,7 +1150,7 @@ void MissionTest::run_max_cmd_test() cmd.content.location.lat = 12345678; cmd.content.location.lng = 23456789; if (!mission.add_cmd(cmd)) { - hal.console->printf_P("failed to add command #%u, library says max is %u\n",(unsigned int)num_commands, (unsigned int)mission.num_commands_max()); + hal.console->printf("failed to add command #%u, library says max is %u\n",(unsigned int)num_commands, (unsigned int)mission.num_commands_max()); failed_to_add = true; }else{ num_commands++; @@ -1160,14 +1160,14 @@ void MissionTest::run_max_cmd_test() // test retrieving commands for (i=0; iprintf_P("failed to retrieve command #%u\n",(unsigned int)i); + hal.console->printf("failed to retrieve command #%u\n",(unsigned int)i); failed_to_read = true; break; }else{ if (cmd.content.location.alt == i) { - hal.console->printf_P("successfully read command #%u\n",(unsigned int)i); + hal.console->printf("successfully read command #%u\n",(unsigned int)i); }else{ - hal.console->printf_P("cmd %u's alt does not match, expected %u but read %u\n", + hal.console->printf("cmd %u's alt does not match, expected %u but read %u\n", (unsigned int)i,(unsigned int)i,(unsigned int)cmd.content.location.alt); } } @@ -1175,15 +1175,15 @@ void MissionTest::run_max_cmd_test() // final success/fail message if (num_commands != mission.num_commands()) { - hal.console->printf_P("\nTest failed! Only wrote %u instead of %u commands",(unsigned int)i,(unsigned int)mission.num_commands_max()); + hal.console->printf("\nTest failed! Only wrote %u instead of %u commands",(unsigned int)i,(unsigned int)mission.num_commands_max()); success = false; } if (failed_to_read) { - hal.console->printf_P("\nTest failed! Only read %u instead of %u commands",(unsigned int)i,(unsigned int)mission.num_commands_max()); + hal.console->printf("\nTest failed! Only read %u instead of %u commands",(unsigned int)i,(unsigned int)mission.num_commands_max()); success = false; } if (success) { - hal.console->printf_P("\nTest Passed! wrote and read back %u commands\n\n",(unsigned int)mission.num_commands_max()); + hal.console->printf("\nTest Passed! wrote and read back %u commands\n\n",(unsigned int)mission.num_commands_max()); } } @@ -1193,8 +1193,8 @@ void MissionTest::setup(void) hal.console->println("AP_Mission library test\n"); // display basic info about command sizes - hal.console->printf_P("Max Num Commands: %d\n",(int)mission.num_commands_max()); - hal.console->printf_P("Command size: %d bytes\n",(int)AP_MISSION_EEPROM_COMMAND_SIZE); + hal.console->printf("Max Num Commands: %d\n",(int)mission.num_commands_max()); + hal.console->printf("Command size: %d bytes\n",(int)AP_MISSION_EEPROM_COMMAND_SIZE); } // loop diff --git a/libraries/AP_Motors/examples/AP_Motors_Time_test/AP_Motors_Time_test.cpp b/libraries/AP_Motors/examples/AP_Motors_Time_test/AP_Motors_Time_test.cpp index 7c9e74207f..4b23d72f5e 100644 --- a/libraries/AP_Motors/examples/AP_Motors_Time_test/AP_Motors_Time_test.cpp +++ b/libraries/AP_Motors/examples/AP_Motors_Time_test/AP_Motors_Time_test.cpp @@ -95,13 +95,13 @@ void motor_order_test() motors.armed(true); for (int8_t i=1; i <= 4; i++) { - hal.console->printf_P("Motor %d\n",(int)i); + hal.console->printf("Motor %d\n",(int)i); int elapsed =0,stop; int start = hal.scheduler->micros(); //Time Test motors.output_test(i, 1150); stop = hal.scheduler->micros(); elapsed = stop - start; - hal.console->printf_P(" Elapsed Time: %dus\n",elapsed); + hal.console->printf(" Elapsed Time: %dus\n",elapsed); hal.scheduler->delay(300); motors.output_test(i, 1000); hal.scheduler->delay(2000); diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp index 2649871d17..c9b9140704 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp +++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp @@ -112,7 +112,7 @@ void motor_order_test() hal.console->println("testing motor order"); motors.armed(true); for (int8_t i=1; i <= AP_MOTORS_MAX_NUM_MOTORS; i++) { - hal.console->printf_P("Motor %d\n",(int)i); + hal.console->printf("Motor %d\n",(int)i); motors.output_test(i, 1150); hal.scheduler->delay(300); motors.output_test(i, 1000); @@ -165,7 +165,7 @@ void stability_test() }; uint32_t testing_array_rows = 32; - hal.console->printf_P("\nTesting stability patch\nThrottle Min:%d Max:%d\n",(int)rc3.radio_min,(int)rc3.radio_max); + hal.console->printf("\nTesting stability patch\nThrottle Min:%d Max:%d\n",(int)rc3.radio_min,(int)rc3.radio_max); // arm motors motors.armed(true); @@ -186,7 +186,7 @@ void stability_test() avg_out = ((hal.rcout->read(0) + hal.rcout->read(1) + hal.rcout->read(2) + hal.rcout->read(3))/4); // display input and output - hal.console->printf_P("R:%5d \tP:%5d \tY:%5d \tT:%5d\tMOT1:%5d \tMOT2:%5d \tMOT3:%5d \tMOT4:%5d \t ThrIn/AvgOut:%5d/%5d\n", + hal.console->printf("R:%5d \tP:%5d \tY:%5d \tT:%5d\tMOT1:%5d \tMOT2:%5d \tMOT3:%5d \tMOT4:%5d \t ThrIn/AvgOut:%5d/%5d\n", (int)roll_in, (int)pitch_in, (int)yaw_in, diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp index 8972d97fc2..544767038f 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_Linux.cpp @@ -188,7 +188,7 @@ void AP_OpticalFlow_Linux::update(void) _update_frontend(state); #if PX4FLOW_DEBUG - hal.console->printf_P("PX4FLOW id:%u qual:%u FlowRateX:%4.2f Y:%4.2f BodyRateX:%4.2f y:%4.2f\n", + hal.console->printf("PX4FLOW id:%u qual:%u FlowRateX:%4.2f Y:%4.2f BodyRateX:%4.2f y:%4.2f\n", (unsigned)state.device_id, (unsigned)state.surface_quality, (double)state.flowRate.x, diff --git a/libraries/AP_Parachute/examples/AP_Parachute_test/AP_Parachute_test.cpp b/libraries/AP_Parachute/examples/AP_Parachute_test/AP_Parachute_test.cpp index 322a5c5b61..d00a216179 100644 --- a/libraries/AP_Parachute/examples/AP_Parachute_test/AP_Parachute_test.cpp +++ b/libraries/AP_Parachute/examples/AP_Parachute_test/AP_Parachute_test.cpp @@ -30,7 +30,7 @@ void setup() void loop() { // print message to user - hal.console->printf_P("this example tests compilation only"); + hal.console->printf("this example tests compilation only"); hal.scheduler->delay(5000); } diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 069f1d0ff8..4df21ec69e 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -1133,16 +1133,16 @@ void AP_Param::show(const AP_Param *ap, const char *s, { switch (type) { case AP_PARAM_INT8: - port->printf_P("%s: %d\n", s, (int)((AP_Int8 *)ap)->get()); + port->printf("%s: %d\n", s, (int)((AP_Int8 *)ap)->get()); break; case AP_PARAM_INT16: - port->printf_P("%s: %d\n", s, (int)((AP_Int16 *)ap)->get()); + port->printf("%s: %d\n", s, (int)((AP_Int16 *)ap)->get()); break; case AP_PARAM_INT32: - port->printf_P("%s: %ld\n", s, (long)((AP_Int32 *)ap)->get()); + port->printf("%s: %ld\n", s, (long)((AP_Int32 *)ap)->get()); break; case AP_PARAM_FLOAT: - port->printf_P("%s: %f\n", s, (double)((AP_Float *)ap)->get()); + port->printf("%s: %f\n", s, (double)((AP_Float *)ap)->get()); break; default: break; @@ -1170,7 +1170,7 @@ void AP_Param::show_all(AP_HAL::BetterStream *port, bool showKeyValues) ap; ap=AP_Param::next_scalar(&token, &type)) { if (showKeyValues) { - port->printf_P("Key %i: Index %i: GroupElement %i : ", token.key, token.idx, token.group_element); + port->printf("Key %i: Index %i: GroupElement %i : ", token.key, token.idx, token.group_element); } show(ap, token, type, port); } @@ -1205,7 +1205,7 @@ void AP_Param::convert_old_parameter(const struct ConversionInfo *info) AP_Param *ap2; ap2 = find(&info->new_name[0], &ptype); if (ap2 == NULL) { - hal.console->printf_P("Unknown conversion '%s'\n", info->new_name); + hal.console->printf("Unknown conversion '%s'\n", info->new_name); return; } @@ -1235,7 +1235,7 @@ void AP_Param::convert_old_parameter(const struct ConversionInfo *info) } } else { // can't do vector<->scalar conversion, or different vector types - hal.console->printf_P("Bad conversion type '%s'\n", info->new_name); + hal.console->printf("Bad conversion type '%s'\n", info->new_name); } } #pragma GCC diagnostic pop diff --git a/libraries/AP_PerfMon/AP_PerfMon.cpp b/libraries/AP_PerfMon/AP_PerfMon.cpp index df9c36b44c..70504daa11 100644 --- a/libraries/AP_PerfMon/AP_PerfMon.cpp +++ b/libraries/AP_PerfMon/AP_PerfMon.cpp @@ -161,8 +161,8 @@ void AP_PerfMon::DisplayResults() hal.console->set_blocking_writes(true); // print table of results - hal.console->printf_P("\nPerfMon elapsed:%lu(ms)\n",(unsigned long)totalTime/1000); - hal.console->printf_P("Fn:\t\tcpu\ttot(ms)\tavg(ms)\tmax(ms)\t#calls\tHz\n"); + hal.console->printf("\nPerfMon elapsed:%lu(ms)\n",(unsigned long)totalTime/1000); + hal.console->printf("Fn:\t\tcpu\ttot(ms)\tavg(ms)\tmax(ms)\t#calls\tHz\n"); for( i=0; iprintf_P("%-10s\t%4.2f\t%lu\t%4.3f\t%4.3f\t%lu\t%4.1f\n", + hal.console->printf("%-10s\t%4.2f\t%lu\t%4.3f\t%4.3f\t%lu\t%4.1f\n", functionNames[j], pct, (unsigned long)time[j]/1000, @@ -192,7 +192,7 @@ void AP_PerfMon::DisplayResults() unExplainedTime = totalTime - sumOfTime; } pct = ((float)unExplainedTime / (float)totalTime) * 100.0f; - hal.console->printf_P("unexpl:\t\t%4.2f\t%lu\n",pct,(unsigned long)unExplainedTime/1000); + hal.console->printf("unexpl:\t\t%4.2f\t%lu\n",pct,(unsigned long)unExplainedTime/1000); // restore to blocking writes if necessary hal.console->set_blocking_writes(false); diff --git a/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp b/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp index 33fb922156..1fad469f68 100644 --- a/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp +++ b/libraries/AP_RangeFinder/examples/RFIND_test/RFIND_test.cpp @@ -23,7 +23,7 @@ void setup() // initialise sensor, delaying to make debug easier hal.scheduler->delay(2000); sonar.init(); - hal.console->printf_P("RangeFinder: %d devices detected\n", sonar.num_sensors()); + hal.console->printf("RangeFinder: %d devices detected\n", sonar.num_sensors()); } void loop() @@ -32,8 +32,8 @@ void loop() hal.scheduler->delay(100); sonar.update(); - hal.console->printf_P("Primary: status %d distance_cm %d \n", (int)sonar.status(), sonar.distance_cm()); - hal.console->printf_P("All: device_0 type %d status %d distance_cm %d, device_1 type %d status %d distance_cm %d\n", + hal.console->printf("Primary: status %d distance_cm %d \n", (int)sonar.status(), sonar.distance_cm()); + hal.console->printf("All: device_0 type %d status %d distance_cm %d, device_1 type %d status %d distance_cm %d\n", (int)sonar._type[0], (int)sonar.status(0), sonar.distance_cm(0), (int)sonar._type[1], (int)sonar.status(1), sonar.distance_cm(1)); } diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp index 8334f34fd9..a5e76cb1e4 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.cpp +++ b/libraries/AP_Scheduler/AP_Scheduler.cpp @@ -73,7 +73,7 @@ void AP_Scheduler::run(uint16_t time_available) if (dt >= interval_ticks*2) { // we've slipped a whole run of this task! if (_debug > 1) { - hal.console->printf_P("Scheduler slip task[%u-%s] (%u/%u/%u)\n", + hal.console->printf("Scheduler slip task[%u-%s] (%u/%u/%u)\n", (unsigned)i, _tasks[i].name, (unsigned)dt, @@ -102,7 +102,7 @@ void AP_Scheduler::run(uint16_t time_available) if (time_taken > _task_time_allowed) { // the event overran! if (_debug > 2) { - hal.console->printf_P("Scheduler overrun task[%u-%s] (%u/%u)\n", + hal.console->printf("Scheduler overrun task[%u-%s] (%u/%u)\n", (unsigned)i, _tasks[i].name, (unsigned)time_taken, diff --git a/libraries/DataFlash/DataFlash_APM1.cpp b/libraries/DataFlash/DataFlash_APM1.cpp index 553255486f..861347b5f9 100644 --- a/libraries/DataFlash/DataFlash_APM1.cpp +++ b/libraries/DataFlash/DataFlash_APM1.cpp @@ -42,7 +42,7 @@ extern const AP_HAL::HAL& hal; //#define ENABLE_FASTSERIAL_DEBUG #ifdef ENABLE_FASTSERIAL_DEBUG - #define serialDebug(fmt, args...) do {hal.console->printf_P( __FUNCTION__ ":%d:" fmt "\n", __LINE__, ##args); } while(0) + #define serialDebug(fmt, args...) do {hal.console->printf( __FUNCTION__ ":%d:" fmt "\n", __LINE__, ##args); } while(0) #else # define serialDebug(fmt, args...) #endif diff --git a/libraries/DataFlash/DataFlash_File.cpp b/libraries/DataFlash/DataFlash_File.cpp index 106cbf7e12..cfc4a7c246 100644 --- a/libraries/DataFlash/DataFlash_File.cpp +++ b/libraries/DataFlash/DataFlash_File.cpp @@ -855,13 +855,13 @@ void DataFlash_File::LogReadProcess(const uint16_t list_entry, */ void DataFlash_File::DumpPageInfo(AP_HAL::BetterStream *port) { - port->printf_P("DataFlash: num_logs=%u\n", + port->printf("DataFlash: num_logs=%u\n", (unsigned)get_num_logs()); } void DataFlash_File::ShowDeviceInfo(AP_HAL::BetterStream *port) { - port->printf_P("DataFlash logs stored in %s\n", + port->printf("DataFlash logs stored in %s\n", _log_directory); } @@ -874,10 +874,10 @@ void DataFlash_File::ListAvailableLogs(AP_HAL::BetterStream *port) uint16_t num_logs = get_num_logs(); if (num_logs == 0) { - port->printf_P("\nNo logs\n\n"); + port->printf("\nNo logs\n\n"); return; } - port->printf_P("\n%u logs\n", (unsigned)num_logs); + port->printf("\n%u logs\n", (unsigned)num_logs); for (uint16_t i=1; i<=num_logs; i++) { uint16_t log_num = _log_num_from_list_entry(i); @@ -886,7 +886,7 @@ void DataFlash_File::ListAvailableLogs(AP_HAL::BetterStream *port) struct stat st; if (stat(filename, &st) == 0) { struct tm *tm = gmtime(&st.st_mtime); - port->printf_P("Log %u in %s of size %u %u/%u/%u %u:%u\n", + port->printf("Log %u in %s of size %u %u/%u/%u %u:%u\n", (unsigned)i, filename, (unsigned)st.st_size, diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 1ae69bb97a..965a833a4c 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -311,7 +311,7 @@ void DataFlash_Backend::_print_log_entry(uint8_t msg_type, } } if (i == _num_types) { - port->printf_P("UNKN, %u\n", (unsigned)msg_type); + port->printf("UNKN, %u\n", (unsigned)msg_type); return; } uint8_t msg_len = PGM_UINT8(&_structures[i].msg_len) - 3; @@ -319,66 +319,66 @@ void DataFlash_Backend::_print_log_entry(uint8_t msg_type, if (!ReadBlock(pkt, msg_len)) { return; } - port->printf_P("%S, ", _structures[i].name); + port->printf("%S, ", _structures[i].name); for (uint8_t ofs=0, fmt_ofs=0; ofsprintf_P("%d", (int)pkt[ofs]); + port->printf("%d", (int)pkt[ofs]); ofs += 1; break; } case 'B': { - port->printf_P("%u", (unsigned)pkt[ofs]); + port->printf("%u", (unsigned)pkt[ofs]); ofs += 1; break; } case 'h': { int16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); - port->printf_P("%d", (int)v); + port->printf("%d", (int)v); ofs += sizeof(v); break; } case 'H': { uint16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); - port->printf_P("%u", (unsigned)v); + port->printf("%u", (unsigned)v); ofs += sizeof(v); break; } case 'i': { int32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); - port->printf_P("%ld", (long)v); + port->printf("%ld", (long)v); ofs += sizeof(v); break; } case 'I': { uint32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); - port->printf_P("%lu", (unsigned long)v); + port->printf("%lu", (unsigned long)v); ofs += sizeof(v); break; } case 'q': { int64_t v; memcpy(&v, &pkt[ofs], sizeof(v)); - port->printf_P("%lld", (long long)v); + port->printf("%lld", (long long)v); ofs += sizeof(v); break; } case 'Q': { uint64_t v; memcpy(&v, &pkt[ofs], sizeof(v)); - port->printf_P("%llu", (unsigned long long)v); + port->printf("%llu", (unsigned long long)v); ofs += sizeof(v); break; } case 'f': { float v; memcpy(&v, &pkt[ofs], sizeof(v)); - port->printf_P("%f", (double)v); + port->printf("%f", (double)v); ofs += sizeof(v); break; } @@ -388,35 +388,35 @@ void DataFlash_Backend::_print_log_entry(uint8_t msg_type, // note that %f here *really* means a single-precision // float, so we lose precision printing this double out // dtoa_engine needed.... - port->printf_P("%f", (double)v); + port->printf("%f", (double)v); ofs += sizeof(v); break; } case 'c': { int16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); - port->printf_P("%.2f", (double)(0.01f*v)); + port->printf("%.2f", (double)(0.01f*v)); ofs += sizeof(v); break; } case 'C': { uint16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); - port->printf_P("%.2f", (double)(0.01f*v)); + port->printf("%.2f", (double)(0.01f*v)); ofs += sizeof(v); break; } case 'e': { int32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); - port->printf_P("%.2f", (double)(0.01f*v)); + port->printf("%.2f", (double)(0.01f*v)); ofs += sizeof(v); break; } case 'E': { uint32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); - port->printf_P("%.2f", (double)(0.01f*v)); + port->printf("%.2f", (double)(0.01f*v)); ofs += sizeof(v); break; } @@ -431,7 +431,7 @@ void DataFlash_Backend::_print_log_entry(uint8_t msg_type, char v[5]; memcpy(&v, &pkt[ofs], sizeof(v)); v[sizeof(v)-1] = 0; - port->printf_P("%s", v); + port->printf("%s", v); ofs += sizeof(v)-1; break; } @@ -439,7 +439,7 @@ void DataFlash_Backend::_print_log_entry(uint8_t msg_type, char v[17]; memcpy(&v, &pkt[ofs], sizeof(v)); v[sizeof(v)-1] = 0; - port->printf_P("%s", v); + port->printf("%s", v); ofs += sizeof(v)-1; break; } @@ -447,7 +447,7 @@ void DataFlash_Backend::_print_log_entry(uint8_t msg_type, char v[65]; memcpy(&v, &pkt[ofs], sizeof(v)); v[sizeof(v)-1] = 0; - port->printf_P("%s", v); + port->printf("%s", v); ofs += sizeof(v)-1; break; } @@ -461,7 +461,7 @@ void DataFlash_Backend::_print_log_entry(uint8_t msg_type, break; } if (ofs < msg_len) { - port->printf_P(", "); + port->printf(", "); } } port->println(); @@ -477,7 +477,7 @@ void DataFlash_Block::_print_log_formats(AP_HAL::BetterStream *port) { for (uint8_t i=0; i<_num_types; i++) { const struct LogStructure *s = &_structures[i]; - port->printf_P("FMT, %u, %u, %S, %S, %S\n", + port->printf("FMT, %u, %u, %S, %S, %S\n", (unsigned)PGM_UINT8(&s->msg_type), (unsigned)PGM_UINT8(&s->msg_len), s->name, s->format, s->labels); @@ -551,9 +551,9 @@ void DataFlash_Block::DumpPageInfo(AP_HAL::BetterStream *port) { for (uint16_t count=1; count<=df_NumPages; count++) { StartRead(count); - port->printf_P("DF page, log file #, log page: %u,\t", (unsigned)count); - port->printf_P("%u,\t", (unsigned)GetFileNumber()); - port->printf_P("%u\n", (unsigned)GetFilePage()); + port->printf("DF page, log file #, log page: %u,\t", (unsigned)count); + port->printf("%u,\t", (unsigned)GetFileNumber()); + port->printf("%u\n", (unsigned)GetFilePage()); } } @@ -567,10 +567,10 @@ void DataFlash_Block::ShowDeviceInfo(AP_HAL::BetterStream *port) return; } ReadManufacturerID(); - port->printf_P("Manufacturer: 0x%02x Device: 0x%04x\n", + port->printf("Manufacturer: 0x%02x Device: 0x%04x\n", (unsigned)df_manufacturer, (unsigned)df_device); - port->printf_P("NumPages: %u PageSize: %u\n", + port->printf("NumPages: %u PageSize: %u\n", (unsigned)df_NumPages+1, (unsigned)df_PageSize); } @@ -586,16 +586,16 @@ void DataFlash_Block::ListAvailableLogs(AP_HAL::BetterStream *port) uint16_t log_end = 0; if (num_logs == 0) { - port->printf_P("\nNo logs\n\n"); + port->printf("\nNo logs\n\n"); return; } - port->printf_P("\n%u logs\n", (unsigned)num_logs); + port->printf("\n%u logs\n", (unsigned)num_logs); for (uint16_t i=num_logs; i>=1; i--) { uint16_t last_log_start = log_start, last_log_end = log_end; uint16_t temp = last_log_num - i + 1; get_log_boundaries(temp, log_start, log_end); - port->printf_P("Log %u, start %u, end %u\n", + port->printf("Log %u, start %u, end %u\n", (unsigned)temp, (unsigned)log_start, (unsigned)log_end); diff --git a/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.cpp b/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.cpp index 073789b381..12b5b57fe4 100644 --- a/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.cpp +++ b/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.cpp @@ -110,7 +110,7 @@ void DataFlashTest::loop(void) void DataFlashTest::print_mode(AP_HAL::BetterStream *port, uint8_t mode) { - port->printf_P("Mode(%u)", (unsigned)mode); + port->printf("Mode(%u)", (unsigned)mode); } /* diff --git a/libraries/GCS_Console/examples/Console/Console.cpp b/libraries/GCS_Console/examples/Console/Console.cpp index dc8921e965..c5ee7d7dca 100644 --- a/libraries/GCS_Console/examples/Console/Console.cpp +++ b/libraries/GCS_Console/examples/Console/Console.cpp @@ -54,7 +54,7 @@ void setup(void) { try_send_statustext(MAVLINK_COMM_0, hello, strlen(hello)); hal.console->backend_open(); - hal.console->printf_P("Hello hal.console\r\n"); + hal.console->printf("Hello hal.console\r\n"); } int i = 0; diff --git a/libraries/GCS_Console/examples/Console/simplegcs.cpp b/libraries/GCS_Console/examples/Console/simplegcs.cpp index 71c8bdffff..60b977d7b3 100644 --- a/libraries/GCS_Console/examples/Console/simplegcs.cpp +++ b/libraries/GCS_Console/examples/Console/simplegcs.cpp @@ -71,7 +71,7 @@ void simplegcs_update(mavlink_channel_t chan) { void handle_message(mavlink_channel_t chan, mavlink_message_t* msg) { - hal.console->printf_P("SimpleGCS Handle Message %d\r\n", msg->msgid); + hal.console->printf("SimpleGCS Handle Message %d\r\n", msg->msgid); switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { diff --git a/libraries/PID/examples/pid/pid.cpp b/libraries/PID/examples/pid/pid.cpp index 163a949cb9..2b825ae56a 100644 --- a/libraries/PID/examples/pid/pid.cpp +++ b/libraries/PID/examples/pid/pid.cpp @@ -31,7 +31,7 @@ void setup() pid.kD(0); pid.imax(0); pid.load_gains(); - hal.console->printf_P( + hal.console->printf( "P %f I %f D %f imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax()); }