Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 23:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity
91ec4e56d5
ardupilot/ArduCopter/setup.pde
Andrew Tridgell 91ec4e56d5 Copter: fixed ESC calibration on Pixhawk 
this ensures motors are armed after safety is pressed, and also gives
print out of channel inputs and outputs on USB console for debug
purposes

Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>
2014-07-29 13:59:28 +10:00

498 lines
13 KiB
Plaintext
Raw Blame History

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if CLI_ENABLED == ENABLED
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
#define WITH_ESC_CALIB
#endif
// Functions called from the setup menu
static int8_t setup_factory (uint8_t argc, const Menu::arg *argv);
static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
#ifdef WITH_ESC_CALIB
static int8_t esc_calib (uint8_t argc, const Menu::arg *argv);
#endif
// Command/function table for the setup menu
const struct Menu::command setup_menu_commands[] PROGMEM = {
// command function called
// ======= ===============
{"reset", setup_factory},
{"show", setup_show},
#ifdef WITH_ESC_CALIB
{"esc_calib", esc_calib},
#endif
};
// 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"));
// Run the setup menu. When the menu exits, we will return to the main menu.
setup_menu.run();
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 board"));
delay(1000);
for (;; ) {
}
// note, cannot actually return here
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;
}
AP_Param::show(param, argv[1].str, type, cliSerial);
return 0;
}
// clear the area
print_blanks(8);
report_version();
report_radio();
report_frame();
report_batt_monitor();
report_flight_modes();
report_ins();
report_compass();
report_optflow();
AP_Param::show_all(cliSerial);
return(0);
}
#ifdef WITH_ESC_CALIB
#define PWM_CALIB_MIN 1000
#define PWM_CALIB_MAX 2000
#define PWM_HIGHEST_MAX 2200
#define PWM_LOWEST_MAX 1200
#define PWM_HIGHEST_MIN 1800
#define PWM_LOWEST_MIN 800
static int8_t
esc_calib(uint8_t argc,const Menu::arg *argv)
{
char c;
unsigned max_channels = 0;
uint32_t set_mask = 0;
uint16_t pwm_high = PWM_CALIB_MAX;
uint16_t pwm_low = PWM_CALIB_MIN;
if (argc < 2) {
cliSerial->printf_P(PSTR("Pls provide Channel Mask\n"
"\tusage: esc_calib 1010 - enables calibration for 2nd and 4th Motor\n"));
return(0);
}
set_mask = strtol (argv[1].str, NULL, 2);
if (set_mask == 0)
cliSerial->printf_P(PSTR("no channels chosen"));
//cliSerial->printf_P(PSTR("\n%d\n"),set_mask);
set_mask<<=1;
/* wait 50 ms */
hal.scheduler->delay(50);
cliSerial->printf_P(PSTR("\nATTENTION, please remove or fix propellers before starting calibration!\n"
"\n"
"Make sure\n"
"\t - that the ESCs are not powered\n"
"\t - that safety is off\n"
"\t - that the controllers are stopped\n"
"\n"
"Do you want to start calibration now: y or n?\n"));
/* wait for user input */
while (1) {
c= cliSerial->read();
if (c == 'y' || c == 'Y') {
break;
} else if (c == 0x03 || c == 0x63 || c == 'q') {
cliSerial->printf_P(PSTR("ESC calibration exited\n"));
return(0);
} else if (c == 'n' || c == 'N') {
cliSerial->printf_P(PSTR("ESC calibration aborted\n"));
return(0);
}
/* rate limit to ~ 20 Hz */
hal.scheduler->delay(50);
}
/* get number of channels available on the device */
max_channels = AP_MOTORS_MAX_NUM_MOTORS;
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
motors.armed(true);
cliSerial->printf_P(PSTR("Outputs armed\n"));
/* wait for user confirmation */
cliSerial->printf_P(PSTR("\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);
while (1) {
/* set max PWM */
for (unsigned i = 0; i < max_channels; i++) {
if (set_mask & 1<<i) {
motors.output_test(i, pwm_high);
}
}
c = cliSerial->read();
if (c == 'c') {
break;
} else if (c == 0x03 || c == 0x63 || c == 'q') {
cliSerial->printf_P(PSTR("ESC calibration exited\n"));
return(0);
}
/* rate limit to ~ 20 Hz */
hal.scheduler->delay(50);
}
cliSerial->printf_P(PSTR("Low PWM set: %d\n"
"\n"
"Hit c+Enter when finished\n"
"\n"), pwm_low);
while (1) {
/* set disarmed PWM */
for (unsigned i = 0; i < max_channels; i++) {
if (set_mask & 1<<i) {
motors.output_test(i, pwm_low);
}
}
c = cliSerial->read();
if (c == 'c') {
break;
} else if (c == 0x03 || c == 0x63 || c == 'q') {
cliSerial->printf_P(PSTR("ESC calibration exited\n"));
return(0);
}
/* rate limit to ~ 20 Hz */
hal.scheduler->delay(50);
}
/* disarm */
motors.armed(false);
cliSerial->printf_P(PSTR("Outputs disarmed\n"));
cliSerial->printf_P(PSTR("ESC calibration finished\n"));
return(0);
}
#endif // WITH_ESC_CALIB
/***************************************************************************/
// CLI reports
/***************************************************************************/
static void report_batt_monitor()
{
cliSerial->printf_P(PSTR("\nBatt Mon:\n"));
print_divider();
if (battery.monitoring() == AP_BATT_MONITOR_DISABLED) print_enabled(false);
if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_ONLY) cliSerial->printf_P(PSTR("volts"));
if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) cliSerial->printf_P(PSTR("volts and cur"));
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
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_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], BIT_IS_SET(g.simple_modes, i));
}
print_blanks(2);
}
void report_optflow()
{
#if OPTFLOW == ENABLED
cliSerial->printf_P(PSTR("OptFlow\n"));
print_divider();
print_enabled(g.optflow_enabled);
print_blanks(2);
#endif // OPTFLOW == ENABLED
}
/***************************************************************************/
// CLI utilities
/***************************************************************************/
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(cliSerial, 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_accel_offsets_and_scaling(void)
{
const Vector3f &accel_offsets = ins.get_accel_offsets();
const 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)
{
const 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);
}
#endif // CLI_ENABLED
// report_compass - displays compass information. Also called by compassmot.pde
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()));
// mag offsets
Vector3f offsets;
for (uint8_t i=0; i<compass.get_count(); i++) {
offsets = compass.get_offsets(i);
// mag offsets
cliSerial->printf_P(PSTR("Mag%d off: %4.4f, %4.4f, %4.4f\n"),
(int)i,
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;
for (uint8_t i=0; i<compass.get_count(); i++) {
motor_compensation = compass.get_motor_compensation(i);
cliSerial->printf_P(PSTR("\nComMot%d: %4.2f, %4.2f, %4.2f\n"),
(int)i,
motor_compensation.x,
motor_compensation.y,
motor_compensation.z);
}
}
print_blanks(1);
}
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);
uint32_t last_print_ms = 0;
while(1) {
motors.armed(true);
motors.enable();
read_radio();
delay(10);
AP_Notify::flags.esc_calibration = true;
motors.throttle_pass_through();
uint32_t now = hal.scheduler->millis();
if (now - last_print_ms > 1000) {
hal.console->printf_P(PSTR("ESC cal input: %u %u %u %u output: %u %u %u %u\n"),
(unsigned)hal.rcin->read(0), (unsigned)hal.rcin->read(1),
(unsigned)hal.rcin->read(2), (unsigned)hal.rcin->read(3),
(unsigned)hal.rcout->read(0), (unsigned)hal.rcout->read(1),
(unsigned)hal.rcout->read(2), (unsigned)hal.rcout->read(3));
last_print_ms = now;
}
}
}
static void report_version()
{
cliSerial->printf_P(PSTR("FW Ver: %d\n"),(int)g.k_format_version);
print_divider();
print_blanks(2);
}
Reference in New Issue View Git Blame Copy Permalink