mirror of https://github.com/ArduPilot/ardupilot
509 lines
13 KiB
C++
509 lines
13 KiB
C++
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#include "Copter.h"
|
|
|
|
#if CLI_ENABLED == ENABLED
|
|
|
|
#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
|
|
|
|
// Command/function table for the setup menu
|
|
static const struct Menu::command setup_menu_commands[] = {
|
|
{"reset", MENU_FUNC(setup_factory)},
|
|
{"show", MENU_FUNC(setup_show)},
|
|
{"set", MENU_FUNC(setup_set)},
|
|
{"esc_calib", MENU_FUNC(esc_calib)},
|
|
};
|
|
|
|
// 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 Copter::setup_mode(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
// Give the user some guidance
|
|
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();
|
|
return 0;
|
|
}
|
|
|
|
// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults).
|
|
// Called by the setup menu 'factoryreset' command.
|
|
int8_t Copter::setup_factory(uint8_t argc, const Menu::arg *argv)
|
|
{
|
|
int16_t c;
|
|
|
|
cliSerial->printf("\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("\nReboot board");
|
|
|
|
delay(1000);
|
|
|
|
for (;; ) {
|
|
}
|
|
// note, cannot actually return here
|
|
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
|
|
int8_t Copter::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("Invalid command. Usage: set <name> <value>\n");
|
|
return 0;
|
|
}
|
|
|
|
param = AP_Param::find(argv[1].str, &p_type);
|
|
if(!param)
|
|
{
|
|
cliSerial->printf("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("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("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("Cannot set parameter of type %d.\n", p_type);
|
|
break;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
// Print the current configuration.
|
|
// Called by the setup menu 'show' command.
|
|
int8_t Copter::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("Parameter not found: '%s'\n", argv[1].str);
|
|
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);
|
|
}
|
|
|
|
int8_t Copter::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("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("no channels chosen");
|
|
//cliSerial->printf("\n%d\n",set_mask);
|
|
set_mask<<=1;
|
|
/* wait 50 ms */
|
|
hal.scheduler->delay(50);
|
|
|
|
|
|
cliSerial->printf("\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("ESC calibration exited\n");
|
|
return(0);
|
|
|
|
} else if (c == 'n' || c == 'N') {
|
|
cliSerial->printf("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("Outputs armed\n");
|
|
|
|
|
|
/* wait for user confirmation */
|
|
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);
|
|
|
|
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("ESC calibration exited\n");
|
|
return(0);
|
|
}
|
|
|
|
/* rate limit to ~ 20 Hz */
|
|
hal.scheduler->delay(50);
|
|
}
|
|
|
|
cliSerial->printf("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("ESC calibration exited\n");
|
|
return(0);
|
|
}
|
|
|
|
/* rate limit to ~ 20 Hz */
|
|
hal.scheduler->delay(50);
|
|
}
|
|
|
|
/* disarm */
|
|
motors.armed(false);
|
|
|
|
cliSerial->printf("Outputs disarmed\n");
|
|
|
|
cliSerial->printf("ESC calibration finished\n");
|
|
|
|
return(0);
|
|
}
|
|
|
|
|
|
/***************************************************************************/
|
|
// CLI reports
|
|
/***************************************************************************/
|
|
|
|
void Copter::report_batt_monitor()
|
|
{
|
|
cliSerial->printf("\nBatt Mon:\n");
|
|
print_divider();
|
|
if (battery.num_instances() == 0) {
|
|
print_enabled(false);
|
|
} else if (!battery.has_current()) {
|
|
cliSerial->printf("volts");
|
|
} else {
|
|
cliSerial->printf("volts and cur");
|
|
}
|
|
print_blanks(2);
|
|
}
|
|
|
|
void Copter::report_frame()
|
|
{
|
|
cliSerial->printf("Frame\n");
|
|
print_divider();
|
|
|
|
#if FRAME_CONFIG == QUAD_FRAME
|
|
cliSerial->printf("Quad frame\n");
|
|
#elif FRAME_CONFIG == TRI_FRAME
|
|
cliSerial->printf("TRI frame\n");
|
|
#elif FRAME_CONFIG == HEXA_FRAME
|
|
cliSerial->printf("Hexa frame\n");
|
|
#elif FRAME_CONFIG == Y6_FRAME
|
|
cliSerial->printf("Y6 frame\n");
|
|
#elif FRAME_CONFIG == OCTA_FRAME
|
|
cliSerial->printf("Octa frame\n");
|
|
#elif FRAME_CONFIG == HELI_FRAME
|
|
cliSerial->printf("Heli frame\n");
|
|
#endif
|
|
|
|
print_blanks(2);
|
|
}
|
|
|
|
void Copter::report_radio()
|
|
{
|
|
cliSerial->printf("Radio\n");
|
|
print_divider();
|
|
// radio
|
|
print_radio_values();
|
|
print_blanks(2);
|
|
}
|
|
|
|
void Copter::report_ins()
|
|
{
|
|
cliSerial->printf("INS\n");
|
|
print_divider();
|
|
|
|
print_gyro_offsets();
|
|
print_accel_offsets_and_scaling();
|
|
print_blanks(2);
|
|
}
|
|
|
|
void Copter::report_flight_modes()
|
|
{
|
|
cliSerial->printf("Flight modes\n");
|
|
print_divider();
|
|
|
|
for(int16_t i = 0; i < 6; i++ ) {
|
|
print_switch(i, (control_mode_t)flight_modes[i].get(), BIT_IS_SET(g.simple_modes, i));
|
|
}
|
|
print_blanks(2);
|
|
}
|
|
|
|
void Copter::report_optflow()
|
|
{
|
|
#if OPTFLOW == ENABLED
|
|
cliSerial->printf("OptFlow\n");
|
|
print_divider();
|
|
|
|
print_enabled(optflow.enabled());
|
|
|
|
print_blanks(2);
|
|
#endif // OPTFLOW == ENABLED
|
|
}
|
|
|
|
/***************************************************************************/
|
|
// CLI utilities
|
|
/***************************************************************************/
|
|
|
|
void Copter::print_radio_values()
|
|
{
|
|
cliSerial->printf("CH1: %d | %d\n", (int)channel_roll->get_radio_min(), (int)channel_roll->get_radio_max());
|
|
cliSerial->printf("CH2: %d | %d\n", (int)channel_pitch->get_radio_min(), (int)channel_pitch->get_radio_max());
|
|
cliSerial->printf("CH3: %d | %d\n", (int)channel_throttle->get_radio_min(), (int)channel_throttle->get_radio_max());
|
|
cliSerial->printf("CH4: %d | %d\n", (int)channel_yaw->get_radio_min(), (int)channel_yaw->get_radio_max());
|
|
cliSerial->printf("CH5: %d | %d\n", (int)g.rc_5.get_radio_min(), (int)g.rc_5.get_radio_max());
|
|
cliSerial->printf("CH6: %d | %d\n", (int)g.rc_6.get_radio_min(), (int)g.rc_6.get_radio_max());
|
|
cliSerial->printf("CH7: %d | %d\n", (int)g.rc_7.get_radio_min(), (int)g.rc_7.get_radio_max());
|
|
cliSerial->printf("CH8: %d | %d\n", (int)g.rc_8.get_radio_min(), (int)g.rc_8.get_radio_max());
|
|
}
|
|
|
|
void Copter::print_switch(uint8_t p, uint8_t m, bool b)
|
|
{
|
|
cliSerial->printf("Pos %d:\t",p);
|
|
print_flight_mode(cliSerial, m);
|
|
cliSerial->printf(",\t\tSimple: ");
|
|
if(b)
|
|
cliSerial->printf("ON\n");
|
|
else
|
|
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("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
|
|
(double)accel_scale.x, // Pitch
|
|
(double)accel_scale.y, // Roll
|
|
(double)accel_scale.z); // YAW
|
|
}
|
|
|
|
void Copter::print_gyro_offsets(void)
|
|
{
|
|
const Vector3f &gyro_offsets = ins.get_gyro_offsets();
|
|
cliSerial->printf("G_off: %4.2f, %4.2f, %4.2f\n",
|
|
(double)gyro_offsets.x,
|
|
(double)gyro_offsets.y,
|
|
(double)gyro_offsets.z);
|
|
}
|
|
|
|
#endif // CLI_ENABLED
|
|
|
|
// report_compass - displays compass information. Also called by compassmot.pde
|
|
void Copter::report_compass()
|
|
{
|
|
cliSerial->printf("Compass\n");
|
|
print_divider();
|
|
|
|
print_enabled(g.compass_enabled);
|
|
|
|
// mag declination
|
|
cliSerial->printf("Mag Dec: %4.4f\n",
|
|
(double)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("Mag%d off: %4.4f, %4.4f, %4.4f\n",
|
|
(int)i,
|
|
(double)offsets.x,
|
|
(double)offsets.y,
|
|
(double)offsets.z);
|
|
}
|
|
|
|
// motor compensation
|
|
cliSerial->print("Motor Comp: ");
|
|
if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_DISABLED ) {
|
|
cliSerial->print("Off\n");
|
|
}else{
|
|
if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_THROTTLE ) {
|
|
cliSerial->print("Throttle");
|
|
}
|
|
if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_CURRENT ) {
|
|
cliSerial->print("Current");
|
|
}
|
|
Vector3f motor_compensation;
|
|
for (uint8_t i=0; i<compass.get_count(); i++) {
|
|
motor_compensation = compass.get_motor_compensation(i);
|
|
cliSerial->printf("\nComMot%d: %4.2f, %4.2f, %4.2f\n",
|
|
(int)i,
|
|
(double)motor_compensation.x,
|
|
(double)motor_compensation.y,
|
|
(double)motor_compensation.z);
|
|
}
|
|
}
|
|
print_blanks(1);
|
|
}
|
|
|
|
void Copter::print_blanks(int16_t num)
|
|
{
|
|
while(num > 0) {
|
|
num--;
|
|
cliSerial->println("");
|
|
}
|
|
}
|
|
|
|
void Copter::print_divider(void)
|
|
{
|
|
for (int i = 0; i < 40; i++) {
|
|
cliSerial->print("-");
|
|
}
|
|
cliSerial->println();
|
|
}
|
|
|
|
void Copter::print_enabled(bool b)
|
|
{
|
|
if(b)
|
|
cliSerial->print("en");
|
|
else
|
|
cliSerial->print("dis");
|
|
cliSerial->print("abled\n");
|
|
}
|
|
|
|
void Copter::report_version()
|
|
{
|
|
cliSerial->printf("FW Ver: %d\n",(int)(g.k_format_version));
|
|
print_divider();
|
|
print_blanks(2);
|
|
}
|