ardupilot/ArduCopter/setup.cpp

509 lines
13 KiB
C++
Raw Normal View History

// -*- 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)
{
2012-08-21 23:19:50 -03:00
// Give the user some guidance
cliSerial->printf("Setup Mode\n\n\n");
2012-08-21 23:19:50 -03:00
// 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 == '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 == '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");
2012-08-21 23:19:50 -03:00
print_divider();
2014-12-03 01:33:06 -04:00
if (battery.num_instances() == 0) {
print_enabled(false);
} else if (!battery.has_current()) {
cliSerial->printf("volts");
2014-12-03 01:33:06 -04:00
} else {
cliSerial->printf("volts and cur");
2014-12-03 01:33:06 -04:00
}
2012-08-21 23:19:50 -03:00
print_blanks(2);
}
void Copter::report_frame()
{
cliSerial->printf("Frame\n");
2012-08-21 23:19:50 -03:00
print_divider();
#if FRAME_CONFIG == QUAD_FRAME
cliSerial->printf("Quad frame\n");
2012-08-21 23:19:50 -03:00
#elif FRAME_CONFIG == TRI_FRAME
cliSerial->printf("TRI frame\n");
2012-08-21 23:19:50 -03:00
#elif FRAME_CONFIG == HEXA_FRAME
cliSerial->printf("Hexa frame\n");
2012-08-21 23:19:50 -03:00
#elif FRAME_CONFIG == Y6_FRAME
cliSerial->printf("Y6 frame\n");
2012-08-21 23:19:50 -03:00
#elif FRAME_CONFIG == OCTA_FRAME
cliSerial->printf("Octa frame\n");
2012-08-21 23:19:50 -03:00
#elif FRAME_CONFIG == HELI_FRAME
cliSerial->printf("Heli frame\n");
2012-08-21 23:19:50 -03:00
#endif
print_blanks(2);
}
void Copter::report_radio()
{
cliSerial->printf("Radio\n");
2012-08-21 23:19:50 -03:00
print_divider();
// radio
print_radio_values();
print_blanks(2);
}
void Copter::report_ins()
{
cliSerial->printf("INS\n");
2012-08-21 23:19:50 -03:00
print_divider();
2012-08-21 23:19:50 -03:00
print_gyro_offsets();
print_accel_offsets_and_scaling();
2012-08-21 23:19:50 -03:00
print_blanks(2);
}
void Copter::report_flight_modes()
{
cliSerial->printf("Flight modes\n");
2012-08-21 23:19:50 -03:00
print_divider();
2012-08-21 23:19:50 -03:00
for(int16_t i = 0; i < 6; i++ ) {
2016-01-25 17:26:20 -04:00
print_switch(i, (control_mode_t)flight_modes[i].get(), BIT_IS_SET(g.simple_modes, i));
2012-08-21 23:19:50 -03:00
}
print_blanks(2);
}
void Copter::report_optflow()
{
#if OPTFLOW == ENABLED
cliSerial->printf("OptFlow\n");
2012-08-21 23:19:50 -03:00
print_divider();
print_enabled(optflow.enabled());
2012-08-21 23:19:50 -03:00
print_blanks(2);
#endif // OPTFLOW == ENABLED
}
2011-12-23 18:41:18 -04:00
/***************************************************************************/
// CLI utilities
/***************************************************************************/
void Copter::print_radio_values()
{
ArduCopter: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
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);
2013-04-20 02:18:22 -03:00
print_flight_mode(cliSerial, m);
cliSerial->printf(",\t\tSimple: ");
2012-08-21 23:19:50 -03:00
if(b)
cliSerial->printf("ON\n");
2012-08-21 23:19:50 -03:00
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
2015-10-25 16:22:31 -03:00
cliSerial->print("Motor Comp: ");
if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_DISABLED ) {
2015-10-25 16:22:31 -03:00
cliSerial->print("Off\n");
}else{
if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_THROTTLE ) {
2015-10-25 16:22:31 -03:00
cliSerial->print("Throttle");
}
if( compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_CURRENT ) {
2015-10-25 16:22:31 -03:00
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)
{
2012-08-21 23:19:50 -03:00
while(num > 0) {
num--;
cliSerial->println("");
2012-08-21 23:19:50 -03:00
}
}
void Copter::print_divider(void)
{
2012-08-21 23:19:50 -03:00
for (int i = 0; i < 40; i++) {
2015-10-25 16:22:31 -03:00
cliSerial->print("-");
2012-08-21 23:19:50 -03:00
}
cliSerial->println();
}
void Copter::print_enabled(bool b)
{
2012-08-21 23:19:50 -03:00
if(b)
2015-10-25 16:22:31 -03:00
cliSerial->print("en");
2012-08-21 23:19:50 -03:00
else
2015-10-25 16:22:31 -03:00
cliSerial->print("dis");
cliSerial->print("abled\n");
}
void Copter::report_version()
{
cliSerial->printf("FW Ver: %d\n",(int)(g.k_format_version));
2012-08-21 23:19:50 -03:00
print_divider();
print_blanks(2);
}