// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #if CLI_ENABLED == ENABLED // 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_accel_scale (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_compassmot (uint8_t argc, const Menu::arg *argv); static int8_t setup_tune (uint8_t argc, const Menu::arg *argv); static int8_t setup_range (uint8_t argc, const Menu::arg *argv); static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); static int8_t setup_optflow (uint8_t argc, const Menu::arg *argv); #if FRAME_CONFIG == HELI_FRAME static int8_t setup_heli (uint8_t argc, const Menu::arg *argv); static int8_t setup_gyro (uint8_t argc, const Menu::arg *argv); #endif static int8_t setup_set (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}, {"level", setup_accel}, {"accel", setup_accel_scale}, {"modes", setup_flightmodes}, {"battery", setup_batt_monitor}, {"sonar", setup_sonar}, {"compass", setup_compass}, {"compassmot", setup_compassmot}, {"tune", setup_tune}, {"range", setup_range}, {"declination", setup_declination}, {"optflow", setup_optflow}, #if FRAME_CONFIG == HELI_FRAME {"heli", setup_heli}, {"gyro", setup_gyro}, #endif {"show", setup_show}, {"set", setup_set} }; // Create the setup menu object. MENU(setup_menu, "setup", setup_menu_commands); // Called from the top-level menu to run the setup menu. static int8_t setup_mode(uint8_t argc, const Menu::arg *argv) { // Give the user some guidance cliSerial->printf_P(PSTR("Setup Mode\n\n\n")); if(g.rc_1.radio_min >= 1300) { delay(1000); cliSerial->printf_P(PSTR("\n!Warning, radio not configured!")); delay(1000); cliSerial->printf_P(PSTR("\n Type 'radio' now.\n\n")); } // Run the setup menu. When the menu exits, we will return to the main menu. setup_menu.run(); return 0; } // Print the current configuration. // Called by the setup menu 'show' command. static int8_t setup_show(uint8_t argc, const Menu::arg *argv) { AP_Param *param; ap_var_type type; //If a parameter name is given as an argument to show, print only that parameter if(argc>=2) { param=AP_Param::find(argv[1].str, &type); if(!param) { cliSerial->printf_P(PSTR("Parameter not found: '%s'\n"), argv[1]); return 0; } //Print differently for different types, and include parameter type in output. switch (type) { case AP_PARAM_INT8: cliSerial->printf_P(PSTR("INT8 %s: %d\n"), argv[1].str, (int)((AP_Int8 *)param)->get()); break; case AP_PARAM_INT16: cliSerial->printf_P(PSTR("INT16 %s: %d\n"), argv[1].str, (int)((AP_Int16 *)param)->get()); break; case AP_PARAM_INT32: cliSerial->printf_P(PSTR("INT32 %s: %ld\n"), argv[1].str, (long)((AP_Int32 *)param)->get()); break; case AP_PARAM_FLOAT: cliSerial->printf_P(PSTR("FLOAT %s: %f\n"), argv[1].str, ((AP_Float *)param)->get()); break; default: cliSerial->printf_P(PSTR("Unhandled parameter type for %s: %d.\n"), argv[1].str, type); break; } return 0; } // clear the area print_blanks(8); report_version(); report_radio(); report_frame(); report_batt_monitor(); report_sonar(); report_flight_modes(); report_ins(); report_compass(); report_optflow(); #if FRAME_CONFIG == HELI_FRAME report_heli(); report_gyro(); #endif AP_Param::show_all(); 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) { int16_t c; cliSerial->printf_P(PSTR("\n'Y' = factory reset, any other key to abort:\n")); do { c = cliSerial->read(); } while (-1 == c); if (('y' != c) && ('Y' != c)) return(-1); AP_Param::erase_all(); cliSerial->printf_P(PSTR("\nReboot APM")); delay(1000); 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) { cliSerial->println_P(PSTR("\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) { //cliSerial->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; cliSerial->printf_P(PSTR("\nMove all controls to extremes. 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(cliSerial->available() > 0) { delay(20); while (cliSerial->read() != -1); /* 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_motors(uint8_t argc, const Menu::arg *argv) { cliSerial->printf_P(PSTR( "Connect battery for this test.\n" "Motors will not spin in channel order (1,2,3,4) but by frame position order.\n" "Front (& right of centerline) motor first, then in clockwise order around frame.\n" "http://code.google.com/p/arducopter/wiki/AC2_Props_2 for demo video.\n" "Remember to disconnect battery after this test.\n" "Any key to exit.\n")); while(1) { delay(20); read_radio(); motors.output_test(); if(cliSerial->available() > 0) { g.esc_calibrate.set_and_save(0); return(0); } } } static int8_t setup_accel(uint8_t argc, const Menu::arg *argv) { ahrs.init(); ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, flash_leds); ins.init_accel(flash_leds); ahrs.set_trim(Vector3f(0,0,0)); // clear out saved trim report_ins(); return(0); } static int8_t setup_accel_scale(uint8_t argc, const Menu::arg *argv) { float trim_roll, trim_pitch; cliSerial->println_P(PSTR("Initialising gyros")); ahrs.init(); ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, flash_leds); AP_InertialSensor_UserInteractStream interact(hal.console); if(ins.calibrate_accel(flash_leds, &interact, trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); } report_ins(); 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 if (!strcmp_P(argv[1].str, PSTR("v"))) { g.frame_orientation.set_and_save(V_FRAME); }else{ cliSerial->printf_P(PSTR("\nOp:[x,+,v]\n")); report_frame(); return 0; } report_frame(); return 0; } static int8_t setup_flightmodes(uint8_t argc, const Menu::arg *argv) { uint8_t _switchPosition = 0; uint8_t _oldSwitchPosition = 0; int8_t mode = 0; cliSerial->printf_P(PSTR("\nMode switch to edit, aileron: select modes, rudder: Simple on/off\n")); print_hit_enter(); while(1) { delay(20); read_radio(); _switchPosition = readSwitch(); // look for control switch change if (_oldSwitchPosition != _switchPosition) { mode = flight_modes[_switchPosition]; mode = constrain(mode, 0, NUM_MODES-1); // update the user print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); // Remember switch position _oldSwitchPosition = _switchPosition; } // look for stick input if (abs(g.rc_1.control_in) > 3000) { mode++; if(mode >= NUM_MODES) mode = 0; // save new mode flight_modes[_switchPosition] = mode; // print new mode print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); delay(500); } // look for stick input if (g.rc_4.control_in > 3000) { g.simple_modes |= (1<<_switchPosition); // print new mode print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); delay(500); } // look for stick input if (g.rc_4.control_in < -3000) { g.simple_modes &= ~(1<<_switchPosition); // print new mode print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); delay(500); } // escape hatch if(cliSerial->available() > 0) { for (mode = 0; mode < 6; mode++) flight_modes[mode].save(); g.simple_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(); return 0; } static int8_t setup_tune(uint8_t argc, const Menu::arg *argv) { g.radio_tuning.set_and_save(argv[1].i); report_tuning(); return 0; } static int8_t setup_range(uint8_t argc, const Menu::arg *argv) { cliSerial->printf_P(PSTR("\nCH 6 Ranges are divided by 1000: [low, high]\n")); g.radio_tuning_low.set_and_save(argv[1].i); g.radio_tuning_high.set_and_save(argv[2].i); report_tuning(); return 0; } 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"))) { clear_offsets(); g.compass_enabled.set_and_save(false); }else{ cliSerial->printf_P(PSTR("\nOp:[on,off]\n")); report_compass(); return 0; } g.compass_enabled.save(); report_compass(); return 0; } // setup_compassmot - sets compass's motor interference parameters static int8_t setup_compassmot(uint8_t argc, const Menu::arg *argv) { int8_t comp_type; // throttle or current based compensation Vector3f compass_base; // compass vector when throttle is zero Vector3f motor_impact; // impact of motors on compass vector Vector3f motor_impact_scaled; // impact of motors on compass vector scaled with throttle Vector3f motor_compensation; // final compensation to be stored to eeprom float throttle_pct; // throttle as a percentage 0.0 ~ 1.0 uint32_t last_run_time; uint8_t print_counter = 49; bool updated = false; // have we updated the compensation vector at least once // default compensation type to use current if possible if( g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT ) { comp_type = AP_COMPASS_MOT_COMP_CURRENT; }else{ comp_type = AP_COMPASS_MOT_COMP_THROTTLE; } // check if user wants throttle compensation if( !strcmp_P(argv[1].str, PSTR("t")) || !strcmp_P(argv[1].str, PSTR("T")) ) { comp_type = AP_COMPASS_MOT_COMP_THROTTLE; } // check if user wants current compensation if( !strcmp_P(argv[1].str, PSTR("c")) || !strcmp_P(argv[1].str, PSTR("C")) ) { comp_type = AP_COMPASS_MOT_COMP_CURRENT; } // check compass is enabled if( !g.compass_enabled ) { cliSerial->print_P(PSTR("compass disabled, exiting")); return 0; } // check if we have a current monitor if( comp_type == AP_COMPASS_MOT_COMP_CURRENT && g.battery_monitoring != BATT_MONITOR_VOLTAGE_AND_CURRENT ) { cliSerial->print_P(PSTR("current monitor disabled, exiting")); return 0; } // initialise compass init_compass(); // disable motor compensation compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED); compass.set_motor_compensation(Vector3f(0,0,0)); // print warning that motors will spin // ask user to raise throttle // inform how to stop test cliSerial->print_P(PSTR("This setup records the impact on the compass of spinning up the motors. The motors will spin!\nHold throttle low, then raise as high as safely possible for 10 sec.\nAt any time you may press any key to exit.\nmeasuring compass vs ")); // inform what type of compensation we are attempting if( comp_type == AP_COMPASS_MOT_COMP_CURRENT ) { cliSerial->print_P(PSTR("CURRENT\n")); }else{ cliSerial->print_P(PSTR("THROTTLE\n")); } // clear out user input while( cliSerial->available() ) { cliSerial->read(); } // disable throttle and battery failsafe g.failsafe_throttle = FS_THR_DISABLED; g.failsafe_battery_enabled = false; // read radio read_radio(); // exit immediately if throttle is not zero if( g.rc_3.control_in != 0 ) { cliSerial->print_P(PSTR("throttle not zero, exiting")); return 0; } // get some initial compass readings last_run_time = millis(); while( millis() - last_run_time < 2000 ) { compass.accumulate(); } compass.read(); // exit immediately if the compass is not healthy if( !compass.healthy ) { cliSerial->print_P(PSTR("compass not healthy, exiting")); return 0; } // store initial x,y,z compass values compass_base.x = compass.mag_x; compass_base.y = compass.mag_y; compass_base.z = compass.mag_z; // initialise motor compensation motor_compensation = Vector3f(0,0,0); // clear out any user input while( cliSerial->available() ) { cliSerial->read(); } // enable motors and pass through throttle motors.enable(); motors.armed(true); motors.output_min(); // initialise run time last_run_time = millis(); // main run while there is no user input and the compass is healthy while(!cliSerial->available() && compass.healthy) { // 50hz loop if( millis() - last_run_time > 20 ) { last_run_time = millis(); // read radio input read_radio(); // pass through throttle to motors motors.throttle_pass_through(); // read some compass values compass.read(); // read current read_battery(); // calculate scaling for throttle throttle_pct = (float)g.rc_3.control_in / 1000.0f; throttle_pct = constrain(throttle_pct,0.0f,1.0f); // if throttle is zero, update base x,y,z values if( throttle_pct == 0.0f ) { compass_base.x = compass_base.x * 0.99f + (float)compass.mag_x * 0.01f; compass_base.y = compass_base.y * 0.99f + (float)compass.mag_y * 0.01f; compass_base.z = compass_base.z * 0.99f + (float)compass.mag_z * 0.01f; // causing printing to happen as soon as throttle is lifted print_counter = 49; }else{ // calculate diff from compass base and scale with throttle motor_impact.x = compass.mag_x - compass_base.x; motor_impact.y = compass.mag_y - compass_base.y; motor_impact.z = compass.mag_z - compass_base.z; // throttle based compensation if( comp_type == AP_COMPASS_MOT_COMP_THROTTLE ) { // scale by throttle motor_impact_scaled = motor_impact / throttle_pct; // adjust the motor compensation to negate the impact motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f; updated = true; }else{ // current based compensation if more than 3amps being drawn motor_impact_scaled = motor_impact / current_amps1; // adjust the motor compensation to negate the impact if drawing over 3amps if( current_amps1 >= 3.0f ) { motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f; updated = true; } } // display output at 1hz if throttle is above zero print_counter++; if(print_counter >= 50) { print_counter = 0; cliSerial->printf_P(PSTR("thr:%d cur:%4.2f mot x:%4.1f y:%4.1f z:%4.1f comp x:%4.2f y:%4.2f z:%4.2f\n"),(int)g.rc_3.control_in, (float)current_amps1, (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z); } } }else{ // grab some compass values compass.accumulate(); } } // stop motors motors.output_min(); motors.armed(false); // clear out any user input while( cliSerial->available() ) { cliSerial->read(); } // print one more time so the last thing printed matches what appears in the report_compass cliSerial->printf_P(PSTR("thr:%d cur:%4.2f mot x:%4.1f y:%4.1f z:%4.1f comp x:%4.2f y:%4.2f z:%4.2f\n"),(int)g.rc_3.control_in, (float)current_amps1, (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z); // set and save motor compensation if( updated ) { compass.motor_compensation_type(comp_type); compass.set_motor_compensation(motor_compensation); compass.save_motor_compensation(); }else{ // compensation vector never updated, report failure cliSerial->printf_P(PSTR("Failed! Compensation disabled. Did you forget to raise the throttle high enough?")); compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED); } // display new motor offsets and save report_compass(); return 0; } static int8_t setup_batt_monitor(uint8_t argc, const Menu::arg *argv) { if (!strcmp_P(argv[1].str, PSTR("off"))) { g.battery_monitoring.set_and_save(0); } else if(argv[1].i > 0 && argv[1].i <= 4) { g.battery_monitoring.set_and_save(argv[1].i); } else { cliSerial->printf_P(PSTR("\nOp: off, 3-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 if (argc > 1 && (argv[1].i >= 0 && argv[1].i <= 3)) { g.sonar_enabled.set_and_save(true); // if you set the sonar type, surely you want it on g.sonar_type.set_and_save(argv[1].i); }else{ cliSerial->printf_P(PSTR("\nOp:[on, off, 0-3]\n")); report_sonar(); return 0; } report_sonar(); return 0; } #if FRAME_CONFIG == HELI_FRAME // Perform heli setup. // Called by the setup menu 'radio' command. static int8_t setup_heli(uint8_t argc, const Menu::arg *argv) { uint8_t active_servo = 0; int16_t value = 0; int16_t temp; int16_t state = 0; // 0 = set rev+pos, 1 = capture min/max int16_t max_roll=0, max_pitch=0, min_collective=0, max_collective=0, min_tail=0, max_tail=0; // initialise swash plate motors.init_swash(); // source swash plate movements directly from radio motors.servo_manual = true; // display initial settings report_heli(); // display help cliSerial->printf_P(PSTR("Instructions:")); print_divider(); cliSerial->printf_P(PSTR("\td\t\tdisplay settings\n")); cliSerial->printf_P(PSTR("\t1~4\t\tselect servo\n")); cliSerial->printf_P(PSTR("\ta or z\t\tmove mid up/down\n")); cliSerial->printf_P(PSTR("\tc\t\tset coll when blade pitch zero\n")); cliSerial->printf_P(PSTR("\tm\t\tset roll, pitch, coll min/max\n")); cliSerial->printf_P(PSTR("\tp\tset pos (i.e. p0 = front, p90 = right)\n")); cliSerial->printf_P(PSTR("\tr\t\treverse servo\n")); cliSerial->printf_P(PSTR("\tu a|d\t\tupdate rate (a=analog servo, d=digital)\n")); cliSerial->printf_P(PSTR("\tt\tset trim (-500 ~ 500)\n")); cliSerial->printf_P(PSTR("\tx\t\texit & save\n")); // start capturing while( value != 'x' ) { // read radio although we don't use it yet read_radio(); // allow swash plate to move motors.output_armed(); // record min/max if( state == 1 ) { if( abs(g.rc_1.control_in) > max_roll ) max_roll = abs(g.rc_1.control_in); if( abs(g.rc_2.control_in) > max_pitch ) max_pitch = abs(g.rc_2.control_in); if( g.rc_3.radio_out < min_collective ) min_collective = g.rc_3.radio_out; if( g.rc_3.radio_out > max_collective ) max_collective = g.rc_3.radio_out; min_tail = min(g.rc_4.radio_out, min_tail); max_tail = max(g.rc_4.radio_out, max_tail); } if( cliSerial->available() ) { value = cliSerial->read(); // process the user's input switch( value ) { case '1': active_servo = CH_1; break; case '2': active_servo = CH_2; break; case '3': active_servo = CH_3; break; case '4': active_servo = CH_4; break; case 'a': case 'A': heli_get_servo(active_servo)->radio_trim += 10; break; case 'c': case 'C': if( g.rc_3.radio_out >= 900 && g.rc_3.radio_out <= 2100 ) { motors.collective_mid = g.rc_3.radio_out; cliSerial->printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)motors.collective_mid); } break; case 'd': case 'D': // display settings report_heli(); break; case 'm': case 'M': if( state == 0 ) { state = 1; // switch to capture min/max mode cliSerial->printf_P(PSTR("Move coll, roll, pitch and tail to extremes, press 'm' when done\n")); // reset servo ranges motors.roll_max = motors.pitch_max = 4500; motors.collective_min = 1000; motors.collective_max = 2000; motors._servo_4->radio_min = 1000; motors._servo_4->radio_max = 2000; // set sensible values in temp variables max_roll = abs(g.rc_1.control_in); max_pitch = abs(g.rc_2.control_in); min_collective = 2000; max_collective = 1000; min_tail = max_tail = abs(g.rc_4.radio_out); }else{ state = 0; // switch back to normal mode // double check values aren't totally terrible if( max_roll <= 1000 || max_pitch <= 1000 || (max_collective - min_collective < 200) || (max_tail - min_tail < 200) || min_tail < 1000 || max_tail > 2000 ) cliSerial->printf_P(PSTR("Invalid min/max captured roll:%d, pitch:%d, collective min: %d max: %d, tail min:%d max:%d\n"),max_roll,max_pitch,min_collective,max_collective,min_tail,max_tail); else{ motors.roll_max = max_roll; motors.pitch_max = max_pitch; motors.collective_min = min_collective; motors.collective_max = max_collective; motors._servo_4->radio_min = min_tail; motors._servo_4->radio_max = max_tail; // reinitialise swash motors.init_swash(); // display settings report_heli(); } } break; case 'p': case 'P': temp = read_num_from_serial(); if( temp >= -360 && temp <= 360 ) { if( active_servo == CH_1 ) motors.servo1_pos = temp; if( active_servo == CH_2 ) motors.servo2_pos = temp; if( active_servo == CH_3 ) motors.servo3_pos = temp; motors.init_swash(); cliSerial->printf_P(PSTR("Servo %d\t\tpos:%d\n"),active_servo+1, temp); } break; case 'r': case 'R': heli_get_servo(active_servo)->set_reverse(!heli_get_servo(active_servo)->get_reverse()); break; case 't': case 'T': temp = read_num_from_serial(); if( temp > 1000 ) temp -= 1500; if( temp > -500 && temp < 500 ) { heli_get_servo(active_servo)->radio_trim = 1500 + temp; motors.init_swash(); cliSerial->printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp); } break; case 'u': case 'U': temp = 0; // delay up to 2 seconds for servo type from user while( !cliSerial->available() && temp < 20 ) { temp++; delay(100); } if( cliSerial->available() ) { value = cliSerial->read(); if( value == 'a' || value == 'A' ) { g.rc_speed.set_and_save(AP_MOTORS_HELI_SPEED_ANALOG_SERVOS); //motors._speed_hz = AP_MOTORS_HELI_SPEED_ANALOG_SERVOS; // need to force this update to take effect immediately cliSerial->printf_P(PSTR("Analog Servo %dhz\n"),(int)g.rc_speed); } if( value == 'd' || value == 'D' ) { g.rc_speed.set_and_save(AP_MOTORS_HELI_SPEED_ANALOG_SERVOS); //motors._speed_hz = AP_MOTORS_HELI_SPEED_ANALOG_SERVOS; // need to force this update to take effect immediately cliSerial->printf_P(PSTR("Digital Servo %dhz\n"),(int)g.rc_speed); } } break; case 'z': case 'Z': heli_get_servo(active_servo)->radio_trim -= 10; break; } } delay(20); } // display final settings report_heli(); // save to eeprom motors._servo_1->save_eeprom(); motors._servo_2->save_eeprom(); motors._servo_3->save_eeprom(); motors._servo_4->save_eeprom(); motors.servo1_pos.save(); motors.servo2_pos.save(); motors.servo3_pos.save(); motors.roll_max.save(); motors.pitch_max.save(); motors.collective_min.save(); motors.collective_max.save(); motors.collective_mid.save(); // return swash plate movements to attitude controller motors.servo_manual = false; return(0); } // setup for external tail gyro (for heli only) static int8_t setup_gyro(uint8_t argc, const Menu::arg *argv) { if (!strcmp_P(argv[1].str, PSTR("on"))) { motors.ext_gyro_enabled.set_and_save(true); // optionally capture the gain if( argc >= 2 && argv[2].i >= 1000 && argv[2].i <= 2000 ) { motors.ext_gyro_gain = argv[2].i; motors.ext_gyro_gain.save(); } } else if (!strcmp_P(argv[1].str, PSTR("off"))) { motors.ext_gyro_enabled.set_and_save(false); // capture gain if user simply provides a number } else if( argv[1].i >= 1000 && argv[1].i <= 2000 ) { motors.ext_gyro_enabled.set_and_save(true); motors.ext_gyro_gain = argv[1].i; motors.ext_gyro_gain.save(); }else{ cliSerial->printf_P(PSTR("\nOp:[on, off] gain\n")); } report_gyro(); return 0; } #endif // FRAME_CONFIG == HELI static void clear_offsets() { Vector3f _offsets(0.0,0.0,0.0); compass.set_offsets(_offsets); compass.save_offsets(); } static int8_t setup_optflow(uint8_t argc, const Menu::arg *argv) { #if OPTFLOW == ENABLED if (!strcmp_P(argv[1].str, PSTR("on"))) { g.optflow_enabled = true; init_optflow(); } else if (!strcmp_P(argv[1].str, PSTR("off"))) { g.optflow_enabled = false; }else{ cliSerial->printf_P(PSTR("\nOp:[on, off]\n")); report_optflow(); return 0; } g.optflow_enabled.save(); report_optflow(); #endif // OPTFLOW == ENABLED return 0; } //Set a parameter to a specified value. It will cast the value to the current type of the //parameter and make sure it fits in case of INT8 and INT16 static int8_t setup_set(uint8_t argc, const Menu::arg *argv) { int8_t value_int8; int16_t value_int16; AP_Param *param; enum ap_var_type p_type; if(argc!=3) { cliSerial->printf_P(PSTR("Invalid command. Usage: set \n")); return 0; } param = AP_Param::find(argv[1].str, &p_type); if(!param) { cliSerial->printf_P(PSTR("Param not found: %s\n"), argv[1].str); return 0; } switch(p_type) { case AP_PARAM_INT8: value_int8 = (int8_t)(argv[2].i); if(argv[2].i!=value_int8) { cliSerial->printf_P(PSTR("Value out of range for type INT8\n")); return 0; } ((AP_Int8*)param)->set_and_save(value_int8); break; case AP_PARAM_INT16: value_int16 = (int16_t)(argv[2].i); if(argv[2].i!=value_int16) { cliSerial->printf_P(PSTR("Value out of range for type INT16\n")); return 0; } ((AP_Int16*)param)->set_and_save(value_int16); break; //int32 and float don't need bounds checking, just use the value provoded by Menu::arg case AP_PARAM_INT32: ((AP_Int32*)param)->set_and_save(argv[2].i); break; case AP_PARAM_FLOAT: ((AP_Float*)param)->set_and_save(argv[2].f); break; default: cliSerial->printf_P(PSTR("Cannot set parameter of type %d.\n"), p_type); break; } return 0; } /***************************************************************************/ // CLI reports /***************************************************************************/ static void report_batt_monitor() { cliSerial->printf_P(PSTR("\nBatt Mon:\n")); print_divider(); if(g.battery_monitoring == BATT_MONITOR_DISABLED) print_enabled(false); if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY) cliSerial->printf_P(PSTR("volts")); if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT) cliSerial->printf_P(PSTR("volts and cur")); print_blanks(2); } static void report_wp(uint8_t index = 255) { if(index == 255) { for(uint8_t i = 0; i < g.command_total; i++) { struct Location temp = get_cmd_with_index(i); print_wp(&temp, i); } }else{ struct Location temp = get_cmd_with_index(index); print_wp(&temp, index); } } static void report_sonar() { cliSerial->printf_P(PSTR("Sonar\n")); print_divider(); print_enabled(g.sonar_enabled.get()); cliSerial->printf_P(PSTR("Type: %d (0=XL, 1=LV, 2=XLL, 3=HRLV)"), (int)g.sonar_type); print_blanks(2); } static void report_frame() { cliSerial->printf_P(PSTR("Frame\n")); print_divider(); #if FRAME_CONFIG == QUAD_FRAME cliSerial->printf_P(PSTR("Quad frame\n")); #elif FRAME_CONFIG == TRI_FRAME cliSerial->printf_P(PSTR("TRI frame\n")); #elif FRAME_CONFIG == HEXA_FRAME cliSerial->printf_P(PSTR("Hexa frame\n")); #elif FRAME_CONFIG == Y6_FRAME cliSerial->printf_P(PSTR("Y6 frame\n")); #elif FRAME_CONFIG == OCTA_FRAME cliSerial->printf_P(PSTR("Octa frame\n")); #elif FRAME_CONFIG == HELI_FRAME cliSerial->printf_P(PSTR("Heli frame\n")); #endif #if FRAME_CONFIG != HELI_FRAME if(g.frame_orientation == X_FRAME) cliSerial->printf_P(PSTR("X mode\n")); else if(g.frame_orientation == PLUS_FRAME) cliSerial->printf_P(PSTR("+ mode\n")); else if(g.frame_orientation == V_FRAME) cliSerial->printf_P(PSTR("V mode\n")); #endif print_blanks(2); } static void report_radio() { cliSerial->printf_P(PSTR("Radio\n")); print_divider(); // radio print_radio_values(); print_blanks(2); } static void report_ins() { cliSerial->printf_P(PSTR("INS\n")); print_divider(); print_gyro_offsets(); print_accel_offsets_and_scaling(); print_blanks(2); } static void report_compass() { cliSerial->printf_P(PSTR("Compass\n")); print_divider(); print_enabled(g.compass_enabled); // mag declination cliSerial->printf_P(PSTR("Mag Dec: %4.4f\n"), degrees(compass.get_declination())); Vector3f offsets = compass.get_offsets(); // mag offsets cliSerial->printf_P(PSTR("Mag off: %4.4f, %4.4f, %4.4f\n"), offsets.x, offsets.y, offsets.z); // motor compensation cliSerial->print_P(PSTR("Motor Comp: ")); if( compass.motor_compensation_type() == AP_COMPASS_MOT_COMP_DISABLED ) { cliSerial->print_P(PSTR("Off\n")); }else{ if( compass.motor_compensation_type() == AP_COMPASS_MOT_COMP_THROTTLE ) { cliSerial->print_P(PSTR("Throttle")); } if( compass.motor_compensation_type() == AP_COMPASS_MOT_COMP_CURRENT ) { cliSerial->print_P(PSTR("Current")); } Vector3f motor_compensation = compass.get_motor_compensation(); cliSerial->printf_P(PSTR("\nComp Vec: %4.2f, %4.2f, %4.2f\n"), motor_compensation.x, motor_compensation.y, motor_compensation.z); } print_blanks(1); } static void report_flight_modes() { cliSerial->printf_P(PSTR("Flight modes\n")); print_divider(); for(int16_t i = 0; i < 6; i++ ) { print_switch(i, flight_modes[i], (g.simple_modes & (1<printf_P(PSTR("OptFlow\n")); print_divider(); print_enabled(g.optflow_enabled); print_blanks(2); #endif // OPTFLOW == ENABLED } #if FRAME_CONFIG == HELI_FRAME static void report_heli() { cliSerial->printf_P(PSTR("Heli\n")); print_divider(); // main servo settings cliSerial->printf_P(PSTR("Servo \tpos \tmin \tmax \trev\n")); cliSerial->printf_P(PSTR("1:\t%d \t%d \t%d \t%d\n"),(int)motors.servo1_pos, (int)motors._servo_1->radio_min, (int)motors._servo_1->radio_max, (int)motors._servo_1->get_reverse()); cliSerial->printf_P(PSTR("2:\t%d \t%d \t%d \t%d\n"),(int)motors.servo2_pos, (int)motors._servo_2->radio_min, (int)motors._servo_2->radio_max, (int)motors._servo_2->get_reverse()); cliSerial->printf_P(PSTR("3:\t%d \t%d \t%d \t%d\n"),(int)motors.servo3_pos, (int)motors._servo_3->radio_min, (int)motors._servo_3->radio_max, (int)motors._servo_3->get_reverse()); cliSerial->printf_P(PSTR("tail:\t\t%d \t%d \t%d\n"), (int)motors._servo_4->radio_min, (int)motors._servo_4->radio_max, (int)motors._servo_4->get_reverse()); cliSerial->printf_P(PSTR("roll max: \t%d\n"), (int)motors.roll_max); cliSerial->printf_P(PSTR("pitch max: \t%d\n"), (int)motors.pitch_max); cliSerial->printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)motors.collective_min, (int)motors.collective_mid, (int)motors.collective_max); // calculate and print servo rate cliSerial->printf_P(PSTR("servo rate:\t%d hz\n"),(int)g.rc_speed); print_blanks(2); } static void report_gyro() { cliSerial->printf_P(PSTR("Gyro:\n")); print_divider(); print_enabled( motors.ext_gyro_enabled ); if( motors.ext_gyro_enabled ) cliSerial->printf_P(PSTR("gain: %d"),(int)motors.ext_gyro_gain); print_blanks(2); } #endif // FRAME_CONFIG == HELI_FRAME /***************************************************************************/ // CLI utilities /***************************************************************************/ /*static void * print_PID(PI * pid) * { * cliSerial->printf_P(PSTR("P: %4.2f, I:%4.2f, IMAX:%ld\n"), * pid->kP(), * pid->kI(), * (long)pid->imax()); * } */ static void print_radio_values() { cliSerial->printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max); cliSerial->printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max); cliSerial->printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max); cliSerial->printf_P(PSTR("CH4: %d | %d\n"), (int)g.rc_4.radio_min, (int)g.rc_4.radio_max); cliSerial->printf_P(PSTR("CH5: %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_max); cliSerial->printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max); cliSerial->printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max); //cliSerial->printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max); } static void print_switch(uint8_t p, uint8_t m, bool b) { cliSerial->printf_P(PSTR("Pos %d:\t"),p); print_flight_mode(m); cliSerial->printf_P(PSTR(",\t\tSimple: ")); if(b) cliSerial->printf_P(PSTR("ON\n")); else cliSerial->printf_P(PSTR("OFF\n")); } static void print_done() { cliSerial->printf_P(PSTR("\nSaved\n")); } static void zero_eeprom(void) { cliSerial->printf_P(PSTR("\nErasing EEPROM\n")); for (uint16_t i = 0; i < EEPROM_MAX_ADDR; i++) { hal.storage->write_byte(i, 0); } cliSerial->printf_P(PSTR("done\n")); } static void print_accel_offsets_and_scaling(void) { Vector3f accel_offsets = ins.get_accel_offsets(); Vector3f accel_scale = ins.get_accel_scale(); cliSerial->printf_P(PSTR("A_off: %4.2f, %4.2f, %4.2f\nA_scale: %4.2f, %4.2f, %4.2f\n"), (float)accel_offsets.x, // Pitch (float)accel_offsets.y, // Roll (float)accel_offsets.z, // YAW (float)accel_scale.x, // Pitch (float)accel_scale.y, // Roll (float)accel_scale.z); // YAW } static void print_gyro_offsets(void) { Vector3f gyro_offsets = ins.get_gyro_offsets(); cliSerial->printf_P(PSTR("G_off: %4.2f, %4.2f, %4.2f\n"), (float)gyro_offsets.x, (float)gyro_offsets.y, (float)gyro_offsets.z); } #if FRAME_CONFIG == HELI_FRAME static RC_Channel * heli_get_servo(int16_t servo_num){ if( servo_num == CH_1 ) return motors._servo_1; if( servo_num == CH_2 ) return motors._servo_2; if( servo_num == CH_3 ) return motors._servo_3; if( servo_num == CH_4 ) return motors._servo_4; return NULL; } // Used to read integer values from the serial port static int16_t read_num_from_serial() { uint8_t index = 0; uint8_t timeout = 0; char data[5] = ""; do { if (cliSerial->available() == 0) { delay(10); timeout++; }else{ data[index] = cliSerial->read(); timeout = 0; index++; } } while (timeout < 5 && index < 5); return atoi(data); } #endif #endif // CLI_ENABLED static void print_blanks(int16_t num) { while(num > 0) { num--; cliSerial->println(""); } } static void print_divider(void) { for (int i = 0; i < 40; i++) { cliSerial->print_P(PSTR("-")); } cliSerial->println(); } static void print_enabled(bool b) { if(b) cliSerial->print_P(PSTR("en")); else cliSerial->print_P(PSTR("dis")); cliSerial->print_P(PSTR("abled\n")); } static void init_esc() { // reduce update rate to motors to 50Hz motors.set_update_rate(50); motors.enable(); motors.armed(true); while(1) { read_radio(); delay(100); dancing_light(); motors.throttle_pass_through(); } } static void print_wp(const struct Location *cmd, uint8_t index) { cliSerial->printf_P(PSTR("cmd#: %d | %d, %d, %d, %ld, %ld, %ld\n"), index, cmd->id, cmd->options, cmd->p1, cmd->alt, cmd->lat, cmd->lng); } static void report_version() { cliSerial->printf_P(PSTR("FW Ver: %d\n"),(int)g.k_format_version); print_divider(); print_blanks(2); } static void report_tuning() { cliSerial->printf_P(PSTR("\nTUNE:\n")); print_divider(); if (g.radio_tuning == 0) { print_enabled(g.radio_tuning.get()); }else{ float low = (float)g.radio_tuning_low.get() / 1000; float high = (float)g.radio_tuning_high.get() / 1000; cliSerial->printf_P(PSTR(" %d, Low:%1.4f, High:%1.4f\n"),(int)g.radio_tuning.get(), low, high); } print_blanks(2); }