From 33a720bbaa0ed0bfc9682ef95c845e472ac8327e Mon Sep 17 00:00:00 2001 From: DrZiplok Date: Sat, 19 Feb 2011 21:04:05 +0000 Subject: [PATCH] Cast AP_Var types for safe printf calls. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1690 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/setup.pde | 190 +++++++++++++++++++-------------------- ArduCopterMega/test.pde | 2 +- 2 files changed, 96 insertions(+), 96 deletions(-) diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index 9abf941f5e..f49ce238b3 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -15,7 +15,7 @@ static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv); static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); static int8_t setup_show (uint8_t argc, const Menu::arg *argv); -// Command/function table for the setup menu +// Command/function table for the setup menu const struct Menu::command setup_menu_commands[] PROGMEM = { // command function called // ======= =============== @@ -61,7 +61,7 @@ setup_show(uint8_t argc, const Menu::arg *argv) uint8_t i; // clear the area print_blanks(8); - + report_radio(); report_frame(); report_current(); @@ -88,14 +88,14 @@ setup_factory(uint8_t argc, const Menu::arg *argv) do { c = Serial.read(); } while (-1 == c); - + if (('y' != c) && ('Y' != c)) return(-1); - + //zero_eeprom(); default_gains(); - - + + // setup default values default_waypoint_info(); default_nav(); @@ -106,7 +106,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv) default_logs(); default_current(); print_done(); - + // finish // ------ return(0); @@ -119,12 +119,12 @@ setup_radio(uint8_t argc, const Menu::arg *argv) { Serial.println("\n\nRadio Setup:"); uint8_t i; - + for(i = 0; i < 100;i++){ delay(20); read_radio(); } - + if(g.rc_1.radio_in < 500){ while(1){ Serial.printf_P(PSTR("\nNo radio; Check connectors.")); @@ -132,7 +132,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv) // stop here } } - + g.rc_1.radio_min = g.rc_1.radio_in; g.rc_2.radio_min = g.rc_2.radio_in; g.rc_3.radio_min = g.rc_3.radio_in; @@ -163,7 +163,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv) Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: ")); while(1){ - + delay(20); // Filters radio input - adjust filters in the radio.pde file // ---------------------------------------------------------- @@ -177,11 +177,11 @@ setup_radio(uint8_t argc, const Menu::arg *argv) g.rc_6.update_min_max(); g.rc_7.update_min_max(); g.rc_8.update_min_max(); - + if(Serial.available() > 0){ //g.rc_3.radio_max += 250; Serial.flush(); - + save_EEPROM_radio(); //delay(100); // double checking @@ -209,9 +209,9 @@ setup_motors(uint8_t argc, const Menu::arg *argv) print_hit_enter(); delay(1000); - + int out_min = g.rc_3.radio_min + 70; - + while(1){ @@ -222,22 +222,22 @@ setup_motors(uint8_t argc, const Menu::arg *argv) motor_out[CH_3] = g.rc_3.radio_min; motor_out[CH_4] = g.rc_3.radio_min; - - + + if(g.frame_type == PLUS_FRAME){ if(g.rc_1.control_in > 0){ motor_out[CH_1] = out_min; Serial.println("0"); - + }else if(g.rc_1.control_in < 0){ motor_out[CH_2] = out_min; Serial.println("1"); } - + if(g.rc_2.control_in > 0){ motor_out[CH_4] = out_min; Serial.println("3"); - + }else if(g.rc_2.control_in < 0){ motor_out[CH_3] = out_min; Serial.println("2"); @@ -264,31 +264,31 @@ setup_motors(uint8_t argc, const Menu::arg *argv) motor_out[CH_1] = out_min; Serial.println("0"); } - + }else if(g.frame_type == TRI_FRAME){ if(g.rc_1.control_in > 0){ motor_out[CH_1] = out_min; - + }else if(g.rc_1.control_in < 0){ motor_out[CH_2] = out_min; } - + if(g.rc_2.control_in > 0){ - motor_out[CH_4] = out_min; + motor_out[CH_4] = out_min; } if(g.rc_4.control_in > 0){ g.rc_4.servo_out = 2000; - + }else if(g.rc_4.control_in < 0){ g.rc_4.servo_out = -2000; } - + g.rc_4.calc_pwm(); motor_out[CH_3] = g.rc_4.radio_out; } - + if(g.rc_3.control_in > 0){ APM_RC.OutputCh(CH_1, g.rc_3.radio_in); APM_RC.OutputCh(CH_2, g.rc_3.radio_in); @@ -301,7 +301,7 @@ setup_motors(uint8_t argc, const Menu::arg *argv) APM_RC.OutputCh(CH_3, motor_out[CH_3]); APM_RC.OutputCh(CH_4, motor_out[CH_4]); } - + if(Serial.available() > 0){ return (0); } @@ -312,7 +312,7 @@ static int8_t setup_accel(uint8_t argc, const Menu::arg *argv) { Serial.printf_P(PSTR("\nHold ArduCopter completely still and level.\n")); - + imu.init_accel(); print_accel_offsets(); @@ -325,12 +325,12 @@ setup_pid(uint8_t argc, const Menu::arg *argv) { if (!strcmp_P(argv[1].str, PSTR("default"))) { default_gains(); - + }else if (!strcmp_P(argv[1].str, PSTR("s_kp"))) { g.pid_stabilize_roll.kP(argv[2].f); g.pid_stabilize_pitch.kP(argv[2].f); save_EEPROM_PID(); - + }else if (!strcmp_P(argv[1].str, PSTR("s_kd"))) { g.stabilize_dampener = argv[2].f; save_EEPROM_PID(); @@ -353,7 +353,7 @@ setup_pid(uint8_t argc, const Menu::arg *argv) }else{ default_gains(); } - + report_gains(); } @@ -362,20 +362,20 @@ static int8_t setup_flightmodes(uint8_t argc, const Menu::arg *argv) { byte switchPosition, oldSwitchPosition, mode; - + Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); print_hit_enter(); trim_radio(); - + while(1){ delay(20); - read_radio(); + read_radio(); switchPosition = readSwitch(); - + // look for control switch change if (oldSwitchPosition != switchPosition){ - + mode = g.flight_modes[switchPosition]; mode = constrain(mode, 0, NUM_MODES-1); @@ -383,12 +383,12 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) print_switch(switchPosition, mode); // Remember switch position - oldSwitchPosition = switchPosition; + oldSwitchPosition = switchPosition; } // look for stick input if (radio_input_switch() == true){ - mode++; + mode++; if(mode >= NUM_MODES) mode = 0; @@ -398,7 +398,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) // print new mode print_switch(switchPosition, mode); } - + // escape hatch if(Serial.available() > 0){ save_EEPROM_flight_modes(); @@ -429,16 +429,16 @@ setup_compass(uint8_t argc, const Menu::arg *argv) if (!strcmp_P(argv[1].str, PSTR("on"))) { g.compass_enabled = true; init_compass(); - + } else if (!strcmp_P(argv[1].str, PSTR("off"))) { g.compass_enabled = false; - + } else { Serial.printf_P(PSTR("\nOptions:[on,off]\n")); report_compass(); return 0; } - + save_EEPROM_mag(); report_compass(); return 0; @@ -452,7 +452,7 @@ setup_frame(uint8_t argc, const Menu::arg *argv) } else if (!strcmp_P(argv[1].str, PSTR("x"))) { g.frame_type = X_FRAME; - + } else if (!strcmp_P(argv[1].str, PSTR("tri"))) { g.frame_type = TRI_FRAME; @@ -464,7 +464,7 @@ setup_frame(uint8_t argc, const Menu::arg *argv) report_frame(); return 0; } - + save_EEPROM_frame(); report_frame(); return 0; @@ -476,20 +476,20 @@ setup_current(uint8_t argc, const Menu::arg *argv) if (!strcmp_P(argv[1].str, PSTR("on"))) { g.current_enabled.set(true); save_EEPROM_mag(); - + } else if (!strcmp_P(argv[1].str, PSTR("off"))) { g.current_enabled.set(false); save_EEPROM_mag(); - + } else if(argv[1].i > 10){ milliamp_hours = argv[1].i; - + } else { Serial.printf_P(PSTR("\nOptions:[on, off, mAh]\n")); report_current(); return 0; } - + save_EEPROM_current(); report_current(); return 0; @@ -508,7 +508,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv) compass.set_orientation(MAGORIENTATION); // set compass's orientation on aircraft compass.set_offsets(0, 0, 0); // set offsets to account for surrounding interference compass.set_declination(ToRad(DECLINATION)); // set local difference between magnetic north and true north - //int counter = 0; + //int counter = 0; float _min[3], _max[3], _offset[3]; while(1){ @@ -536,15 +536,15 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv) offset[0] = -(_max[0] + _min[0]) / 2; offset[1] = -(_max[1] + _min[1]) / 2; offset[2] = -(_max[2] + _min[2]) / 2; - - // display all to user + + // display all to user Serial.printf_P(PSTR("Heading: ")); Serial.print(ToDeg(compass.heading)); Serial.print(" \t("); Serial.print(compass.mag_x); Serial.print(","); Serial.print(compass.mag_y); - Serial.print(","); + Serial.print(","); Serial.print(compass.mag_z); Serial.print(")\t offsets("); Serial.print(offset[0]); @@ -553,18 +553,18 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv) Serial.print(","); Serial.print(offset[2]); Serial.println(")"); - + if(Serial.available() > 0){ - + //mag_offset_x = offset[0]; //mag_offset_y = offset[1]; //mag_offset_z = offset[2]; - + //save_EEPROM_mag_offset(); - + // set offsets to account for surrounding interference //compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z); - + report_compass(); break; } @@ -578,7 +578,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv) /***************************************************************************/ void default_waypoint_info() -{ +{ g.waypoint_radius = 4; //TODO: Replace this quick fix with a real way to define wp_radius g.loiter_radius = 30; //TODO: Replace this quick fix with a real way to define loiter_radius save_EEPROM_waypoint_info(); @@ -646,7 +646,7 @@ void default_logs() // convenience macro for testing LOG_* and setting LOGBIT_* #define LOGBIT(_s) (LOG_ ## _s ? LOGBIT_ ## _s : 0) - g.log_bitmask = + g.log_bitmask = LOGBIT(ATTITUDE_FAST) | LOGBIT(ATTITUDE_MED) | LOGBIT(GPS) | @@ -658,7 +658,7 @@ void default_logs() LOGBIT(CMD) | LOGBIT(CURRENT); #undef LOGBIT - + save_EEPROM_logs(); } @@ -671,7 +671,7 @@ default_gains() g.pid_acro_rate_roll.kI(ACRO_RATE_ROLL_I); g.pid_acro_rate_roll.kD(0); g.pid_acro_rate_roll.imax(ACRO_RATE_ROLL_IMAX * 100); - + g.pid_acro_rate_pitch.kP(ACRO_RATE_PITCH_P); g.pid_acro_rate_pitch.kI(ACRO_RATE_PITCH_I); g.pid_acro_rate_pitch.kD(0); @@ -688,7 +688,7 @@ default_gains() g.pid_stabilize_roll.kI(STABILIZE_ROLL_I); g.pid_stabilize_roll.kD(0); g.pid_stabilize_roll.imax(STABILIZE_ROLL_IMAX * 100); - + g.pid_stabilize_pitch.kP(STABILIZE_PITCH_P); g.pid_stabilize_pitch.kI(STABILIZE_PITCH_I); g.pid_stabilize_pitch.kD(0); @@ -704,7 +704,7 @@ default_gains() // custom dampeners // roll pitch g.stabilize_dampener = STABILIZE_DAMPENER; - + //yaw g.hold_yaw_dampener = HOLD_YAW_DAMPENER; @@ -729,7 +729,7 @@ default_gains() g.pid_sonar_throttle.kI(THROTTLE_SONAR_I); g.pid_sonar_throttle.kD(THROTTLE_SONAR_D); g.pid_sonar_throttle.imax(THROTTLE_SONAR_IMAX); - + save_EEPROM_PID(); } @@ -759,7 +759,7 @@ void report_frame() Serial.printf_P(PSTR("Frame\n")); print_divider(); - + if(g.frame_type == X_FRAME) Serial.printf_P(PSTR("X ")); else if(g.frame_type == PLUS_FRAME) @@ -806,10 +806,10 @@ void report_gains() print_PID(&g.pid_stabilize_pitch); Serial.printf_P(PSTR("yaw:\n")); print_PID(&g.pid_yaw); - - Serial.printf_P(PSTR("Stabilize dampener: %4.3f\n"), g.stabilize_dampener); - Serial.printf_P(PSTR("Yaw Dampener: %4.3f\n\n"), g.hold_yaw_dampener); - + + Serial.printf_P(PSTR("Stabilize dampener: %4.3f\n"), (float)g.stabilize_dampener); + Serial.printf_P(PSTR("Yaw Dampener: %4.3f\n\n"), (float)g.hold_yaw_dampener); + // Nav Serial.printf_P(PSTR("Nav:\nlat:\n")); print_PID(&g.pid_nav_lat); @@ -832,9 +832,9 @@ void report_xtrack() Serial.printf_P(PSTR("XTRACK: %4.2f\n" "XTRACK angle: %d\n" "PITCH_MAX: %ld"), - g.crosstrack_gain, - g.crosstrack_entry_angle, - g.pitch_max); + (float)g.crosstrack_gain, + (int)g.crosstrack_entry_angle, + (long)g.pitch_max); print_blanks(1); } @@ -850,11 +850,11 @@ void report_throttle() "cruise: %d\n" "failsafe_enabled: %d\n" "failsafe_value: %d"), - g.throttle_min, - g.throttle_max, - g.throttle_cruise, - g.throttle_failsafe_enabled, - g.throttle_failsafe_value); + (int)g.throttle_min, + (int)g.throttle_max, + (int)g.throttle_cruise, + (int)g.throttle_failsafe_enabled, + (int)g.throttle_failsafe_value); print_blanks(1); } @@ -871,7 +871,7 @@ void report_imu() void report_compass() { - print_blanks(2); + print_blanks(2); Serial.printf_P(PSTR("Compass\n")); print_divider(); @@ -880,11 +880,11 @@ void report_compass() //read_EEPROM_compass_offset(); print_enabled(g.compass_enabled); - + // mag declination - Serial.printf_P(PSTR("Mag Delination: %4.4f\n"), + Serial.printf_P(PSTR("Mag Delination: %4.4f\n"), degrees(compass.get_declination())); - + Vector3f offsets = compass.get_offsets(); // mag offsets @@ -898,11 +898,11 @@ void report_compass() void report_flight_modes() { - print_blanks(2); + print_blanks(2); Serial.printf_P(PSTR("Flight modes\n")); print_divider(); read_EEPROM_flight_modes(); - + for(int i = 0; i < 6; i++ ){ print_switch(i, g.flight_modes[i]); } @@ -913,23 +913,23 @@ void report_flight_modes() // CLI utilities /***************************************************************************/ -void +void print_PID(PID * pid) { Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"), pid->kP(), pid->kI(), pid->kD(), (long)pid->imax()); } -void +void print_radio_values() { - Serial.printf_P(PSTR("CH1: %d | %d\n"), g.rc_1.radio_min, g.rc_1.radio_max); - Serial.printf_P(PSTR("CH2: %d | %d\n"), g.rc_2.radio_min, g.rc_2.radio_max); - Serial.printf_P(PSTR("CH3: %d | %d\n"), g.rc_3.radio_min, g.rc_3.radio_max); - Serial.printf_P(PSTR("CH4: %d | %d\n"), g.rc_4.radio_min, g.rc_4.radio_max); - Serial.printf_P(PSTR("CH5: %d | %d\n"), g.rc_5.radio_min, g.rc_5.radio_max); - Serial.printf_P(PSTR("CH6: %d | %d\n"), g.rc_6.radio_min, g.rc_6.radio_max); - Serial.printf_P(PSTR("CH7: %d | %d\n"), g.rc_7.radio_min, g.rc_7.radio_max); - Serial.printf_P(PSTR("CH8: %d | %d\n"), g.rc_8.radio_min, g.rc_8.radio_max); + Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max); + Serial.printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max); + Serial.printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max); + Serial.printf_P(PSTR("CH4: %d | %d\n"), (int)g.rc_4.radio_min, (int)g.rc_4.radio_max); + Serial.printf_P(PSTR("CH5: %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_max); + Serial.printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max); + Serial.printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max); + Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max); } void @@ -969,7 +969,7 @@ boolean radio_input_switch(void) { static byte bouncer; - + if (abs(g.rc_1.radio_in - g.rc_1.radio_trim) > 200) bouncer = 10; diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index fd902832d7..e3ad9ed93d 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -795,7 +795,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv) current_loc.alt, next_WP.alt, altitude_error, - g.throttle_cruise, + (int)g.throttle_cruise, g.rc_3.servo_out); /*