// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // Functions called from the setup menu static int8_t setup_radio (uint8_t argc, const Menu::arg *argv); static int8_t setup_motors (uint8_t argc, const Menu::arg *argv); static int8_t setup_accel (uint8_t argc, const Menu::arg *argv); static int8_t setup_frame (uint8_t argc, const Menu::arg *argv); static int8_t setup_factory (uint8_t argc, const Menu::arg *argv); static int8_t setup_erase (uint8_t argc, const Menu::arg *argv); static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv); static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv); static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv); static int8_t setup_compass (uint8_t argc, const Menu::arg *argv); 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_esc (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 const struct Menu::command setup_menu_commands[] PROGMEM = { // command function called // ======= =============== {"erase", setup_erase}, {"reset", setup_factory}, {"radio", setup_radio}, {"frame", setup_frame}, {"motors", setup_motors}, {"esc", setup_esc}, {"level", setup_accel}, {"modes", setup_flightmodes}, {"battery", setup_batt_monitor}, {"sonar", setup_sonar}, {"compass", setup_compass}, {"offsets", setup_mag_offset}, {"declination", setup_declination}, {"show", setup_show} }; // Create the setup menu object. MENU(setup_menu, "setup", setup_menu_commands); // Called from the top-level menu to run the setup menu. int8_t setup_mode(uint8_t argc, const Menu::arg *argv) { // Give the user some guidance Serial.printf_P(PSTR("Setup Mode\n\n\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" //"and then the 'radio' command to configure for your radio.\n" //"\n")); if(g.rc_1.radio_min >= 1300){ delay(1000); Serial.printf_P(PSTR("\n!Warning, your radio is not configured!")); delay(1000); Serial.printf_P(PSTR("\n Type 'radio' to configure now.\n\n")); } // Run the setup menu. When the menu exits, we will return to the main menu. setup_menu.run(); } // Print the current configuration. // Called by the setup menu 'show' command. static int8_t setup_show(uint8_t argc, const Menu::arg *argv) { // clear the area print_blanks(8); report_version(); report_radio(); report_frame(); report_batt_monitor(); report_sonar(); report_gains(); report_xtrack(); report_throttle(); report_flight_modes(); report_imu(); report_compass(); AP_Var_menu_show(argc, argv); return(0); } // Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults). // Called by the setup menu 'factoryreset' command. static int8_t setup_factory(uint8_t argc, const Menu::arg *argv) { int c; Serial.printf_P(PSTR("\n'Y' + Enter to factory reset, any other key to abort:\n")); do { c = Serial.read(); } while (-1 == c); if (('y' != c) && ('Y' != c)) return(-1); AP_Var::erase_all(); Serial.printf_P(PSTR("\nFACTORY RESET complete - reboot APM")); delay(1000); //default_log_bitmask(); //default_gains(); for (;;) { } // note, cannot actually return here return(0); } // Perform radio setup. // Called by the setup menu 'radio' command. static int8_t 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.")); delay(1000); // 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; g.rc_4.radio_min = g.rc_4.radio_in; g.rc_5.radio_min = g.rc_5.radio_in; g.rc_6.radio_min = g.rc_6.radio_in; g.rc_7.radio_min = g.rc_7.radio_in; g.rc_8.radio_min = g.rc_8.radio_in; g.rc_1.radio_max = g.rc_1.radio_in; g.rc_2.radio_max = g.rc_2.radio_in; g.rc_3.radio_max = g.rc_3.radio_in; g.rc_4.radio_max = g.rc_4.radio_in; g.rc_5.radio_max = g.rc_5.radio_in; g.rc_6.radio_max = g.rc_6.radio_in; g.rc_7.radio_max = g.rc_7.radio_in; g.rc_8.radio_max = g.rc_8.radio_in; g.rc_1.radio_trim = g.rc_1.radio_in; g.rc_2.radio_trim = g.rc_2.radio_in; g.rc_4.radio_trim = g.rc_4.radio_in; // 3 is not trimed g.rc_5.radio_trim = 1500; g.rc_6.radio_trim = 1500; g.rc_7.radio_trim = 1500; g.rc_8.radio_trim = 1500; 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 // ---------------------------------------------------------- read_radio(); g.rc_1.update_min_max(); g.rc_2.update_min_max(); g.rc_3.update_min_max(); g.rc_4.update_min_max(); g.rc_5.update_min_max(); g.rc_6.update_min_max(); g.rc_7.update_min_max(); g.rc_8.update_min_max(); if(Serial.available() > 0){ delay(20); Serial.flush(); g.rc_1.save_eeprom(); g.rc_2.save_eeprom(); g.rc_3.save_eeprom(); g.rc_4.save_eeprom(); g.rc_5.save_eeprom(); g.rc_6.save_eeprom(); g.rc_7.save_eeprom(); g.rc_8.save_eeprom(); print_done(); break; } } report_radio(); return(0); } static int8_t setup_esc(uint8_t argc, const Menu::arg *argv) { Serial.printf_P(PSTR("\nUnplug, then plug-in battery; Calibrate ESCs.\n Press Enter to cancel.\n")); g.esc_calibrate.set_and_save(1); while(1){ delay(20); if(Serial.available() > 0){ g.esc_calibrate.set_and_save(0); return(0); } } } void init_esc() { g.esc_calibrate.set_and_save(0); while(1){ read_radio(); delay(100); update_esc_light(); APM_RC.OutputCh(CH_1, g.rc_3.radio_in); APM_RC.OutputCh(CH_2, g.rc_3.radio_in); APM_RC.OutputCh(CH_3, g.rc_3.radio_in); APM_RC.OutputCh(CH_4, g.rc_3.radio_in); APM_RC.OutputCh(CH_7, g.rc_3.radio_in); APM_RC.OutputCh(CH_8, g.rc_3.radio_in); #if FRAME_CONFIG == OCTA_FRAME APM_RC.OutputCh(CH_10, g.rc_3.radio_in); APM_RC.OutputCh(CH_11, g.rc_3.radio_in); #endif } } static int8_t setup_motors(uint8_t argc, const Menu::arg *argv) { while(1){ output_motor_test(); if(Serial.available() > 0){ g.esc_calibrate.set_and_save(0); return(0); } } } static int8_t setup_accel(uint8_t argc, const Menu::arg *argv) { imu.init_accel(); print_accel_offsets(); report_imu(); return(0); } static int8_t setup_frame(uint8_t argc, const Menu::arg *argv) { if (!strcmp_P(argv[1].str, PSTR("x"))) { g.frame_orientation.set_and_save(X_FRAME); } else if (!strcmp_P(argv[1].str, PSTR("p"))) { g.frame_orientation.set_and_save(PLUS_FRAME); } else if (!strcmp_P(argv[1].str, PSTR("+"))) { g.frame_orientation.set_and_save(PLUS_FRAME); }else{ Serial.printf_P(PSTR("\nOptions:[x,+]\n")); report_frame(); return 0; } report_frame(); return 0; } 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(); while(1){ delay(20); read_radio(); switchPosition = readSwitch(); // look for control switch change if (_oldSwitchPosition != switchPosition){ mode = g.flight_modes[switchPosition]; mode = constrain(mode, 0, NUM_MODES-1); // update the user print_switch(switchPosition, mode); // Remember switch position _oldSwitchPosition = switchPosition; } // look for stick input if (radio_input_switch() == true){ mode++; if(mode >= NUM_MODES) mode = 0; // save new mode g.flight_modes[switchPosition] = mode; // print new mode print_switch(switchPosition, mode); } // escape hatch if(Serial.available() > 0){ g.flight_modes.save(); print_done(); report_flight_modes(); return (0); } } } static int8_t setup_declination(uint8_t argc, const Menu::arg *argv) { compass.set_declination(radians(argv[1].f)); report_compass(); } static int8_t setup_erase(uint8_t argc, const Menu::arg *argv) { zero_eeprom(); return 0; } static int8_t setup_compass(uint8_t argc, const Menu::arg *argv) { if (!strcmp_P(argv[1].str, PSTR("on"))) { g.compass_enabled.set_and_save(true); init_compass(); } else if (!strcmp_P(argv[1].str, PSTR("off"))) { g.compass_enabled.set_and_save(false); }else{ Serial.printf_P(PSTR("\nOptions:[on,off]\n")); report_compass(); return 0; } g.compass_enabled.save(); report_compass(); return 0; } static int8_t setup_batt_monitor(uint8_t argc, const Menu::arg *argv) { if(argv[1].i >= 0 && argv[1].i <= 4){ g.battery_monitoring.set_and_save(argv[1].i); } else { Serial.printf_P(PSTR("\nOptions: 0-4")); } report_batt_monitor(); return 0; } static int8_t setup_sonar(uint8_t argc, const Menu::arg *argv) { if (!strcmp_P(argv[1].str, PSTR("on"))) { g.sonar_enabled.set_and_save(true); } else if (!strcmp_P(argv[1].str, PSTR("off"))) { g.sonar_enabled.set_and_save(false); }else{ Serial.printf_P(PSTR("\nOptions:[on, off]\n")); report_sonar(); return 0; } report_sonar(); return 0; } static int8_t setup_mag_offset(uint8_t argc, const Menu::arg *argv) { print_hit_enter(); init_compass(); float _min[3], _max[3]; Vector3f _offsets; Vector3f compass_mag; while(1){ static float min[3], _max[3], offset[3]; if (millis() - fast_loopTimer > 100) { delta_ms_fast_loop = millis() - fast_loopTimer; fast_loopTimer = millis(); G_Dt = (float)delta_ms_fast_loop / 1000.f; compass.read(); compass.calculate(0, 0); // roll = 0, pitch = 0 for this example compass_mag = compass.get_offsets(); // capture min _min[0] = min(_min[0], compass_mag.x); _min[1] = min(_min[1], compass_mag.y); _min[2] = min(_min[2], compass_mag.z); // capture max _max[0] = max(_max[0], compass_mag.x); _max[1] = max(_max[1], compass_mag.y); _max[2] = max(_max[2], compass_mag.z); // calculate offsets _offsets.x = -(_max[0] + _min[0]) / 2; _offsets.y = -(_max[1] + _min[1]) / 2; _offsets.z = -(_max[2] + _min[2]) / 2; // display all to user Serial.printf_P(PSTR("Heading: %u, \t (%4.4f, %4.4f, %4.4f) (%4.4f, %4.4f, %4.4f)\n"), (uint16_t)wrap_360(ToDeg(compass.heading)), compass_mag.x, compass_mag.y, compass_mag.z, _offsets.x, _offsets.y, _offsets.z); if(Serial.available() > 0){ compass.set_offsets(_offsets); //compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z); report_compass(); break; } } } } /***************************************************************************/ // CLI defaults /***************************************************************************/ void default_log_bitmask() { // convenience macro for testing LOG_* and setting LOGBIT_* #define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0) g.log_bitmask = LOGBIT(ATTITUDE_FAST) | LOGBIT(ATTITUDE_MED) | LOGBIT(GPS) | LOGBIT(PM) | LOGBIT(CTUN) | LOGBIT(NTUN) | LOGBIT(MODE) | LOGBIT(RAW) | LOGBIT(CMD) | LOGBIT(CURRENT); #undef LOGBIT g.log_bitmask.save(); } /***************************************************************************/ // CLI reports /***************************************************************************/ void report_batt_monitor() { Serial.printf_P(PSTR("\nBatt Mointor\n")); print_divider(); if(g.battery_monitoring == 0) print_enabled(false); if(g.battery_monitoring == 1) Serial.printf_P(PSTR("3 cells")); if(g.battery_monitoring == 2) Serial.printf_P(PSTR("4 cells")); if(g.battery_monitoring == 3) Serial.printf_P(PSTR("batt volts")); if(g.battery_monitoring == 4) Serial.printf_P(PSTR("volts and cur")); print_blanks(2); } void report_wp(byte index = 255) { if(index == 255){ for(byte i = 0; i <= g.waypoint_total; i++){ struct Location temp = get_command_with_index(i); print_wp(&temp, i); } }else{ struct Location temp = get_command_with_index(index); print_wp(&temp, index); } } void print_wp(struct Location *cmd, byte index) { Serial.printf_P(PSTR("command #: %d id:%d op:%d p1:%d p2:%ld p3:%ld p4:%ld \n"), (int)index, (int)cmd->id, (int)cmd->options, (int)cmd->p1, cmd->alt, cmd->lat, cmd->lng); } void report_gps() { Serial.printf_P(PSTR("\nGPS\n")); print_divider(); print_enabled(GPS_enabled); print_blanks(2); } void report_sonar() { g.sonar_enabled.load(); Serial.printf_P(PSTR("Sonar\n")); print_divider(); print_enabled(g.sonar_enabled.get()); print_blanks(2); } void report_version() { Serial.printf_P(PSTR("FW Version %d\n"),(int)g.format_version.get()); print_divider(); print_blanks(2); } void report_frame() { Serial.printf_P(PSTR("Frame\n")); print_divider(); #if FRAME_CONFIG == QUAD_FRAME Serial.printf_P(PSTR("Quad frame\n")); #elif FRAME_CONFIG == TRI_FRAME Serial.printf_P(PSTR("TRI frame\n")); #elif FRAME_CONFIG == HEXA_FRAME Serial.printf_P(PSTR("Hexa frame\n")); #elif FRAME_CONFIG == Y6_FRAME Serial.printf_P(PSTR("Y6 frame\n")); #elif FRAME_CONFIG == OCTA_FRAME Serial.printf_P(PSTR("Octa frame\n")); #endif if(g.frame_orientation == X_FRAME) Serial.printf_P(PSTR("X mode\n")); else if(g.frame_orientation == PLUS_FRAME) Serial.printf_P(PSTR("+ mode\n")); print_blanks(2); } void report_radio() { Serial.printf_P(PSTR("Radio\n")); print_divider(); // radio print_radio_values(); print_blanks(2); } void report_gains() { Serial.printf_P(PSTR("Gains\n")); print_divider(); // Acro Serial.printf_P(PSTR("Acro:\nroll:\n")); print_PID(&g.pid_acro_rate_roll); Serial.printf_P(PSTR("pitch:\n")); print_PID(&g.pid_acro_rate_pitch); Serial.printf_P(PSTR("yaw:\n")); print_PID(&g.pid_acro_rate_yaw); // Stabilize Serial.printf_P(PSTR("\nStabilize:\nroll:\n")); print_PID(&g.pid_stabilize_roll); Serial.printf_P(PSTR("pitch:\n")); print_PID(&g.pid_stabilize_pitch); Serial.printf_P(PSTR("yaw:\n")); print_PID(&g.pid_yaw); //Serial.printf_P(PSTR("Stab D: %4.3f\n"), (float)g.stabilize_dampener); //Serial.printf_P(PSTR("Yaw D: %4.3f\n\n"), (float)g.hold_yaw_dampener); // Nav Serial.printf_P(PSTR("Nav:\nlat:\n")); print_PID(&g.pid_nav_lat); Serial.printf_P(PSTR("long:\n")); print_PID(&g.pid_nav_lon); Serial.printf_P(PSTR("baro throttle:\n")); print_PID(&g.pid_baro_throttle); Serial.printf_P(PSTR("sonar throttle:\n")); print_PID(&g.pid_sonar_throttle); print_blanks(2); } void report_xtrack() { Serial.printf_P(PSTR("XTrack\n")); print_divider(); // radio Serial.printf_P(PSTR("XTRACK: %4.2f\n" "XTRACK angle: %d\n" "PITCH_MAX: %ld"), (float)g.crosstrack_gain, (int)g.crosstrack_entry_angle, (long)g.pitch_max); print_blanks(2); } void report_throttle() { Serial.printf_P(PSTR("Throttle\n")); print_divider(); Serial.printf_P(PSTR("min: %d\n" "max: %d\n" "cruise: %d\n" "failsafe_enabled: %d\n" "failsafe_value: %d"), (int)g.throttle_min, (int)g.throttle_max, (int)g.throttle_cruise, (int)g.throttle_fs_enabled, (int)g.throttle_fs_value); print_blanks(2); } void report_imu() { Serial.printf_P(PSTR("IMU\n")); print_divider(); print_gyro_offsets(); print_accel_offsets(); print_blanks(2); } void report_compass() { Serial.printf_P(PSTR("Compass\n")); print_divider(); print_enabled(g.compass_enabled); // mag declination Serial.printf_P(PSTR("Mag Dec: %4.4f\n"), degrees(compass.get_declination())); Vector3f offsets = compass.get_offsets(); // mag offsets Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f"), offsets.x, offsets.y, offsets.z); print_blanks(2); } void report_flight_modes() { Serial.printf_P(PSTR("Flight modes\n")); print_divider(); for(int i = 0; i < 6; i++ ){ print_switch(i, g.flight_modes[i]); } print_blanks(2); } /***************************************************************************/ // CLI utilities /***************************************************************************/ void print_PID(PID * pid) { Serial.printf_P(PSTR("P: %4.2f, I:%4.2f, D:%4.2f, IMAX:%ld\n"), pid->kP(), pid->kI(), pid->kD(), (long)pid->imax()); } void print_radio_values() { 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 print_switch(byte p, byte m) { Serial.printf_P(PSTR("Pos %d: "),p); Serial.println(flight_mode_strings[m]); } void print_done() { Serial.printf_P(PSTR("\nSaved Settings\n\n")); } void print_blanks(int num) { while(num > 0){ num--; Serial.println(""); } } void print_divider(void) { for (int i = 0; i < 40; i++) { Serial.print("-"); } Serial.println(""); } // read at 50Hz bool radio_input_switch(void) { static int8_t bouncer = 0; if (int16_t(g.rc_1.radio_in - g.rc_1.radio_trim) > 100) { bouncer = 10; } if (int16_t(g.rc_1.radio_in - g.rc_1.radio_trim) < -100) { bouncer = -10; } if (bouncer >0) { bouncer --; } if (bouncer <0) { bouncer ++; } if (bouncer == 1 || bouncer == -1) { return bouncer; }else{ return 0; } } void zero_eeprom(void) { byte b; Serial.printf_P(PSTR("\nErasing EEPROM\n")); for (int i = 0; i < EEPROM_MAX_ADDR; i++) { eeprom_write_byte((uint8_t *) i, b); } Serial.printf_P(PSTR("done\n")); } void print_enabled(boolean b) { if(b) Serial.printf_P(PSTR("en")); else Serial.printf_P(PSTR("dis")); Serial.printf_P(PSTR("abled\n")); } void print_accel_offsets(void) { Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"), (float)imu.ax(), (float)imu.ay(), (float)imu.az()); } void print_gyro_offsets(void) { Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"), (float)imu.gx(), (float)imu.gy(), (float)imu.gz()); }