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.
This commit is contained in:
Lucas De Marchi 2015-10-25 18:10:41 -02:00 committed by Randy Mackay
parent 7ceffb1e2b
commit 20c6ffc5e3
66 changed files with 382 additions and 382 deletions

View File

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

View File

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

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -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;
}
}

View File

@ -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,

View File

@ -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));
}
}

View File

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

View File

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

View File

@ -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);
}
}

View File

@ -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;
}
}

View File

@ -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 <name> <value>\n");
cliSerial->printf("Invalid command. Usage: set <name> <value>\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; i<compass.get_count(); i++) {
offsets = compass.get_offsets(i);
// mag offsets
cliSerial->printf_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; i<compass.get_count(); i++) {
motor_compensation = compass.get_motor_compensation(i);
cliSerial->printf_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);
}

View File

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

View File

@ -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

View File

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

View File

@ -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);
}
}

View File

@ -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

View File

@ -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;
}
}

View File

@ -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

View File

@ -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),

View File

@ -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

View File

@ -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);
}
}

View File

@ -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
}

View File

@ -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:

View File

@ -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);

View File

@ -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

View File

@ -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(;;);
}

View File

@ -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(;;);
}

View File

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

View File

@ -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);
}
}

View File

@ -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],

View File

@ -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 */

View File

@ -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)

View File

@ -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);

View File

@ -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

View File

@ -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

View File

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

View File

@ -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(;;);
}

View File

@ -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; k<num_gyros; k++) {
if (!converged[k]) {
hal.console->printf_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

View File

@ -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();
}

View File

@ -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();
}

View File

@ -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);

View File

@ -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();
}

View File

@ -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);
}
}

View File

@ -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),

View File

@ -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 ",

View File

@ -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));
}

View File

@ -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

View File

@ -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);
}
}

View File

@ -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; i<mission.num_commands(); i++) {
@ -657,23 +657,23 @@ void MissionTest::print_mission()
mission.read_cmd_from_storage(i,cmd);
// print command position in list and mavlink id
hal.console->printf_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; i<num_commands; i++) {
if (!mission.read_cmd_from_storage(i,cmd)) {
hal.console->printf_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

View File

@ -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);

View File

@ -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,

View File

@ -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,

View File

@ -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);
}

View File

@ -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

View File

@ -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; i<nextFuncNum; i++ ) {
j=order[i];
sumOfTime += time[j];
@ -176,7 +176,7 @@ void AP_PerfMon::DisplayResults()
hz = numCalls[j]/(totalTime/1000000);
pct = ((float)time[j] / (float)totalTime) * 100.0f;
hal.console->printf_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);

View File

@ -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));
}

View File

@ -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,

View File

@ -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

View File

@ -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,

View File

@ -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; ofs<msg_len; fmt_ofs++) {
char fmt = PGM_UINT8(&_structures[i].format[fmt_ofs]);
switch (fmt) {
case 'b': {
port->printf_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);

View File

@ -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);
}
/*

View File

@ -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;

View File

@ -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:
{

View File

@ -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());
}