aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

New updated CLI for setup. Cleaner and better defaults and reporting of values.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1516 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-01-20 07:11:21 +00:00
parent 05f8162d81
commit baea026e59
6 changed files with 459 additions and 335 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopterMega/ArduCopterMega.pde
View File

@ -226,8 +226,8 @@ float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 +
float current_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
float current_amps;
float current_total;
boolean current_sensor = false;
int milliamp_hours;
boolean current_enabled = false;
// Magnetometer variables
// ----------------------
@ -573,7 +573,7 @@ void medium_loop()
// This case controls the slow loop
//---------------------------------
case 4:
if (current_sensor){
if (current_enabled){
read_current();
}

50
ArduCopterMega/EEPROM.pde
View File

@ -9,25 +9,22 @@ void read_EEPROM_startup(void)
{
read_EEPROM_PID();
read_EEPROM_frame();
read_EEPROM_configs();
read_EEPROM_throttle();
read_EEPROM_logs();
read_EEPROM_flight_modes();
read_EEPROM_waypoint_info();
imu.load_gyro_eeprom();
imu.load_accel_eeprom();
read_EEPROM_alt_RTL();
read_EEPROM_current();
// magnatometer
read_EEPROM_mag();
read_EEPROM_mag_declination();
read_EEPROM_mag_offset();
read_EEPROM_compass();
read_EEPROM_compass_declination();
read_EEPROM_compass_offset();
}
void read_EEPROM_airstart_critical(void)
{
int16_t temp = 0;
imu.load_gyro_eeprom();
imu.load_accel_eeprom();
{
// Read the home location
//-----------------------
home = get_wp_with_index(0);
@ -196,21 +193,24 @@ void save_EEPROM_mag_declination(void)
write_EE_compressed_float(mag_declination, EE_MAG_DECLINATION, 1);
}
void read_EEPROM_mag_declination(void)
void read_EEPROM_compass_declination(void)
{
mag_declination = read_EE_compressed_float(EE_MAG_DECLINATION, 1);
}
/********************************************************************************/
void save_EEPROM_current_sensor(void)
void save_EEPROM_current(void)
{
eeprom_write_byte((uint8_t *) EE_CURRENT_SENSOR, current_sensor);
eeprom_write_byte((uint8_t *) EE_CURRENT_SENSOR, current_enabled);
eeprom_write_word((uint16_t *) EE_CURRENT_MAH, milliamp_hours);
}
void read_EEPROM_current_sensor(void)
void read_EEPROM_current(void)
{
current_sensor = eeprom_read_byte((uint8_t *) EE_CURRENT_SENSOR);
current_enabled = eeprom_read_byte((uint8_t *) EE_CURRENT_SENSOR);
milliamp_hours = eeprom_read_word((uint16_t *) EE_CURRENT_MAH);
}
/********************************************************************************/
@ -222,7 +222,7 @@ void save_EEPROM_mag_offset(void)
write_EE_compressed_float(mag_offset_z, EE_MAG_Z, 2);
}
void read_EEPROM_mag_offset(void)
void read_EEPROM_compass_offset(void)
{
mag_offset_x = read_EE_compressed_float(EE_MAG_X, 2);
mag_offset_y = read_EE_compressed_float(EE_MAG_Y, 2);
@ -231,7 +231,7 @@ void read_EEPROM_mag_offset(void)
/********************************************************************************/
void read_EEPROM_mag(void)
void read_EEPROM_compass(void)
{
compass_enabled = eeprom_read_byte((uint8_t *) EE_COMPASS);
}
@ -297,7 +297,7 @@ void save_EEPROM_radio(void)
/********************************************************************************/
// configs are the basics
void read_EEPROM_configs(void)
void read_EEPROM_throttle(void)
{
throttle_min = eeprom_read_word((uint16_t *) EE_THROTTLE_MIN);
throttle_max = eeprom_read_word((uint16_t *) EE_THROTTLE_MAX);
@ -305,10 +305,9 @@ void read_EEPROM_configs(void)
throttle_failsafe_enabled = eeprom_read_byte((byte *) EE_THROTTLE_FAILSAFE);
throttle_failsafe_action = eeprom_read_byte((byte *) EE_THROTTLE_FAILSAFE_ACTION);
throttle_failsafe_value = eeprom_read_word((uint16_t *) EE_THROTTLE_FS_VALUE);
log_bitmask = eeprom_read_word((uint16_t *) EE_LOG_BITMASK);
}
void save_EEPROM_configs(void)
void save_EEPROM_throttle(void)
{
eeprom_write_word((uint16_t *) EE_THROTTLE_MIN, throttle_min);
eeprom_write_word((uint16_t *) EE_THROTTLE_MAX, throttle_max);
@ -316,6 +315,17 @@ void save_EEPROM_configs(void)
eeprom_write_byte((byte *) EE_THROTTLE_FAILSAFE, throttle_failsafe_enabled);
eeprom_write_byte((byte *) EE_THROTTLE_FAILSAFE_ACTION,throttle_failsafe_action);
eeprom_write_word((uint16_t *) EE_THROTTLE_FS_VALUE, throttle_failsafe_value);
}
/********************************************************************************/
void read_EEPROM_logs(void)
{
log_bitmask = eeprom_read_word((uint16_t *) EE_LOG_BITMASK);
}
void save_EEPROM_logs(void)
{
eeprom_write_word((uint16_t *) EE_LOG_BITMASK, log_bitmask);
}

2
ArduCopterMega/Log.pde
View File

@ -175,7 +175,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
} else {
log_bitmask &= ~bits;
}
save_EEPROM_configs(); // XXX this is a bit heavyweight...
save_EEPROM_logs(); // XXX this is a bit heavyweight...
return(0);
}

2
ArduCopterMega/defines.h
View File

@ -312,7 +312,7 @@
#define EE_GND_ALT 0x11C
#define EE_AP_OFFSET 0x11E
#define EE_CURRENT_SENSOR 0x127
#define EE_CURRENT_MAH 0x0128
// log
#define EE_LAST_LOG_PAGE 0xE00
#define EE_LAST_LOG_NUM 0xE02

732
ArduCopterMega/setup.pde
View File

@ -9,6 +9,7 @@ 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_pid (uint8_t argc, const Menu::arg *argv);
static int8_t setup_frame (uint8_t argc, const Menu::arg *argv);
static int8_t setup_current (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);
@ -26,6 +27,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
{"level", setup_accel},
{"modes", setup_flightmodes},
{"frame", setup_frame},
{"current", setup_current},
{"compass", setup_compass},
{"mag_offset", setup_mag_offset},
{"declination", setup_declination},
@ -57,120 +59,18 @@ static int8_t
setup_show(uint8_t argc, const Menu::arg *argv)
{
uint8_t i;
print_blanks(10);
Serial.printf_P(PSTR("Radio\n"));
print_divider();
// radio
read_EEPROM_radio();
print_radio_values();
// frame
print_blanks(2);
Serial.printf_P(PSTR("Frame\n"));
print_divider();
read_EEPROM_frame();
if(frame_type == X_FRAME)
Serial.printf_P(PSTR("X "));
else if(frame_type == PLUS_FRAME)
Serial.printf_P(PSTR("Plus "));
Serial.printf_P(PSTR("(%d)\n"), (int)frame_type);
print_blanks(2);
read_EEPROM_current_sensor();
Serial.printf_P(PSTR("Current Sensor "));
if(current_sensor){
Serial.printf_P(PSTR("enabled\n"));
} else {
Serial.printf_P(PSTR("disabled\n"));
}
print_blanks(2);
Serial.printf_P(PSTR("Gains\n"));
print_divider();
read_EEPROM_PID();
// Acro
Serial.printf_P(PSTR("Acro:\nroll:\n"));
print_PID(&pid_acro_rate_roll);
Serial.printf_P(PSTR("pitch:\n"));
print_PID(&pid_acro_rate_pitch);
Serial.printf_P(PSTR("yaw:\n"));
print_PID(&pid_acro_rate_yaw);
// Stabilize
Serial.printf_P(PSTR("\nStabilize:\nroll:\n"));
print_PID(&pid_stabilize_roll);
Serial.printf_P(PSTR("pitch:\n"));
print_PID(&pid_stabilize_pitch);
Serial.printf_P(PSTR("yaw:\n"));
print_PID(&pid_yaw);
Serial.printf_P(PSTR("Stabilize dampener: %4.3f\n"), stabilize_dampener);
Serial.printf_P(PSTR("Yaw Dampener: %4.3f\n\n"), hold_yaw_dampener);
// Nav
Serial.printf_P(PSTR("Nav:\nlat:\n"));
print_PID(&pid_nav_lat);
Serial.printf_P(PSTR("Nav:\nlong:\n"));
print_PID(&pid_nav_lon);
Serial.printf_P(PSTR("baro throttle:\n"));
print_PID(&pid_baro_throttle);
Serial.printf_P(PSTR("sonar throttle:\n"));
print_PID(&pid_sonar_throttle);
Serial.println(" ");
print_blanks(2);
Serial.printf_P(PSTR("User Configs\n"));
print_divider();
// Crosstrack
read_EEPROM_nav();
Serial.printf_P(PSTR("XTRACK: %4.2f\n"), x_track_gain);
Serial.printf_P(PSTR("XTRACK angle: %d\n"), x_track_angle);
Serial.printf_P(PSTR("PITCH_MAX: %d\n"), pitch_max);
// User Configs
read_EEPROM_configs();
Serial.printf_P(PSTR("throttle_min: %d\n"), throttle_min);
Serial.printf_P(PSTR("throttle_max: %d\n"), throttle_max);
Serial.printf_P(PSTR("throttle_cruise: %d\n"), throttle_cruise);
Serial.printf_P(PSTR("throttle_failsafe_enabled: %d\n"), throttle_failsafe_enabled);
Serial.printf_P(PSTR("throttle_failsafe_value: %d\n"), throttle_failsafe_value);
Serial.printf_P(PSTR("log_bitmask: %d\n"), log_bitmask);
print_blanks(2);
Serial.printf_P(PSTR("IMU\n"));
print_divider();
imu.print_gyro_offsets();
imu.print_accel_offsets();
print_blanks(2);
Serial.printf_P(PSTR("Compass\n"));
print_divider();
if(compass_enabled)
Serial.printf_P(PSTR("en"));
else
Serial.printf_P(PSTR("dis"));
Serial.printf_P(PSTR("abled\n\n"));
// mag declination
read_EEPROM_mag_declination();
Serial.printf_P(PSTR("Mag Delination: "));
Serial.println(mag_declination,2);
// mag offsets
Serial.printf_P(PSTR("Mag offsets: "));
Serial.print(mag_offset_x, 2);
Serial.printf_P(PSTR(", "));
Serial.print(mag_offset_y, 2);
Serial.printf_P(PSTR(", "));
Serial.println(mag_offset_z, 2);
// clear the area
print_blanks(8);
report_radio();
report_frame();
report_current();
report_gains();
report_xtrack();
report_throttle();
report_flight_modes();
report_imu();
report_compass();
return(0);
}
@ -179,14 +79,7 @@ setup_show(uint8_t argc, const Menu::arg *argv)
static int8_t
setup_factory(uint8_t argc, const Menu::arg *argv)
{
/*
saves:
save_EEPROM_waypoint_info();
save_EEPROM_nav();
save_EEPROM_flight_modes();
save_EEPROM_configs();
*/
uint8_t i;
int c;
@ -198,62 +91,20 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
if (('y' != c) && ('Y' != c))
return(-1);
//Serial.printf_P(PSTR("\nFACTORY RESET\n\n"));
//zero_eeprom();
setup_pid(0 ,NULL);
default_gains();
wp_radius = 4; //TODO: Replace this quick fix with a real way to define wp_radius
loiter_radius = 30; //TODO: Replace this quick fix with a real way to define loiter_radius
save_EEPROM_waypoint_info();
// nav control
x_track_gain = XTRACK_GAIN * 100;
x_track_angle = XTRACK_ENTRY_ANGLE * 100;
pitch_max = PITCH_MAX * 100;
save_EEPROM_nav();
// alt hold
alt_to_hold = -1;
save_EEPROM_alt_RTL();
// default to a + configuration
frame_type = PLUS_FRAME;
save_EEPROM_frame();
flight_modes[0] = FLIGHT_MODE_1;
flight_modes[1] = FLIGHT_MODE_2;
flight_modes[2] = FLIGHT_MODE_3;
flight_modes[3] = FLIGHT_MODE_4;
flight_modes[4] = FLIGHT_MODE_5;
flight_modes[5] = FLIGHT_MODE_6;
save_EEPROM_flight_modes();
// user configs
throttle_min = THROTTLE_MIN;
throttle_max = THROTTLE_MAX;
throttle_cruise = THROTTLE_CRUISE;
throttle_failsafe_enabled = THROTTLE_FAILSAFE;
throttle_failsafe_action = THROTTLE_FAILSAFE_ACTION;
throttle_failsafe_value = THROTTLE_FS_VALUE;
// convenience macro for testing LOG_* and setting LOGBIT_*
#define LOGBIT(_s) (LOG_ ## _s ? LOGBIT_ ## _s : 0)
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
save_EEPROM_configs();
// setup default values
default_waypoint_info();
default_nav();
default_alt_hold();
default_frame();
default_flight_modes();
default_throttle();
default_logs();
default_current();
print_done();
// finish
@ -340,13 +191,17 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
break;
}
}
report_radio();
return(0);
}
static int8_t
setup_motors(uint8_t argc, const Menu::arg *argv)
{
report_frame();
init_rc_in();
// read the radio to set trims
// ---------------------------
trim_radio();
@ -357,18 +212,8 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
int out_min = rc_3.radio_min + 70;
if(frame_type == PLUS_FRAME){
Serial.printf_P(PSTR("PLUS"));
}else if(frame_type == X_FRAME){
Serial.printf_P(PSTR("X"));
}else if(frame_type == TRI_FRAME){
Serial.printf_P(PSTR("TRI"));
}
Serial.printf_P(PSTR(" Frame\n"));
while(1){
delay(20);
read_radio();
@ -466,101 +311,19 @@ static int8_t
setup_accel(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\nHold ArduCopter completely still and level.\n"));
/*
imu.init_gyro();
print_gyro();
imu.load_gyro_eeprom();
print_gyro();
*/
imu.init_accel();
imu.print_accel_offsets();
//imu.load_accel_eeprom();
//print_accel();
return(0);
}
static int8_t
setup_accel_flat(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\nClear Accel offsets.\n"));
imu.zero_accel();
imu.print_accel_offsets();
report_imu();
return(0);
}
static int8_t
setup_pid(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\nSetting default PID gains\n"));
// acro, angular rate
pid_acro_rate_roll.kP(ACRO_RATE_ROLL_P);
pid_acro_rate_roll.kI(ACRO_RATE_ROLL_I);
pid_acro_rate_roll.kD(ACRO_RATE_ROLL_D);
pid_acro_rate_roll.imax(ACRO_RATE_ROLL_IMAX * 100);
pid_acro_rate_pitch.kP(ACRO_RATE_PITCH_P);
pid_acro_rate_pitch.kI(ACRO_RATE_PITCH_I);
pid_acro_rate_pitch.kD(ACRO_RATE_PITCH_D);
pid_acro_rate_pitch.imax(ACRO_RATE_PITCH_IMAX * 100);
pid_acro_rate_yaw.kP(ACRO_RATE_YAW_P);
pid_acro_rate_yaw.kI(ACRO_RATE_YAW_I);
pid_acro_rate_yaw.kD(ACRO_RATE_YAW_D);
pid_acro_rate_yaw.imax(ACRO_RATE_YAW_IMAX * 100);
// stabilize, angle error
pid_stabilize_roll.kP(STABILIZE_ROLL_P);
pid_stabilize_roll.kI(STABILIZE_ROLL_I);
pid_stabilize_roll.kD(STABILIZE_ROLL_D);
pid_stabilize_roll.imax(STABILIZE_ROLL_IMAX * 100);
pid_stabilize_pitch.kP(STABILIZE_PITCH_P);
pid_stabilize_pitch.kI(STABILIZE_PITCH_I);
pid_stabilize_pitch.kD(STABILIZE_PITCH_D);
pid_stabilize_pitch.imax(STABILIZE_PITCH_IMAX * 100);
// YAW hold
pid_yaw.kP(YAW_P);
pid_yaw.kI(YAW_I);
pid_yaw.kD(YAW_D);
pid_yaw.imax(YAW_IMAX * 100);
// custom dampeners
// roll pitch
stabilize_dampener = STABILIZE_DAMPENER;
//yaw
hold_yaw_dampener = HOLD_YAW_DAMPENER;
// navigation
pid_nav_lat.kP(NAV_P);
pid_nav_lat.kI(NAV_I);
pid_nav_lat.kD(NAV_D);
pid_nav_lat.imax(NAV_IMAX * 100);
pid_nav_lon.kP(NAV_P);
pid_nav_lon.kI(NAV_I);
pid_nav_lon.kD(NAV_D);
pid_nav_lon.imax(NAV_IMAX * 100);
pid_baro_throttle.kP(THROTTLE_BARO_P);
pid_baro_throttle.kI(THROTTLE_BARO_I);
pid_baro_throttle.kD(THROTTLE_BARO_D);
pid_baro_throttle.imax(THROTTLE_BARO_IMAX * 100);
pid_sonar_throttle.kP(THROTTLE_SONAR_P);
pid_sonar_throttle.kI(THROTTLE_SONAR_I);
pid_sonar_throttle.kD(THROTTLE_SONAR_D);
pid_sonar_throttle.imax(THROTTLE_SONAR_IMAX * 100);
save_EEPROM_PID();
print_done();
default_gains();
report_gains();
}
static int8_t
@ -608,6 +371,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
if(Serial.available() > 0){
save_EEPROM_flight_modes();
print_done();
report_flight_modes();
return (0);
}
}
@ -618,10 +382,7 @@ setup_declination(uint8_t argc, const Menu::arg *argv)
{
mag_declination = argv[1].f;
save_EEPROM_mag_declination();
read_EEPROM_mag_declination();
Serial.printf_P(PSTR("\nsaved: "));
Serial.println(argv[1].f, 2);
report_compass();
}
static int8_t
@ -634,65 +395,71 @@ setup_erase(uint8_t argc, const Menu::arg *argv)
static int8_t
setup_compass(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Compass "));
if (!strcmp_P(argv[1].str, PSTR("on"))) {
Serial.printf_P(PSTR("Enabled\n"));
compass_enabled = true;
init_compass();
save_EEPROM_mag();
print_done();
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
Serial.printf_P(PSTR("Disabled\n"));
compass_enabled = false;
save_EEPROM_mag();
print_done();
} else {
if (compass_enabled) {
Serial.printf_P(PSTR("en"));
} else {
Serial.printf_P(PSTR("dis"));
}
Serial.printf_P(PSTR("abled\n\n"));
Serial.printf_P(PSTR("\nUsage:\nEnabled =>compass 1\nDisabled =>compass 0\n\n"));
Serial.printf_P(PSTR("\nOptions:[on,off]\n"));
report_compass();
return 0;
}
save_EEPROM_mag();
report_compass();
return 0;
}
static int8_t
setup_frame(uint8_t argc, const Menu::arg *argv)
{
if(argv[1].i < 1){
Serial.printf_P(PSTR("\nUsage:\nPlus frame =>frame 1\nX frame =>frame 2\nTRI frame =>frame 3\n\n"));
return 0;
}
print_done();
if(argv[1].i == 1){
Serial.printf_P(PSTR("Plus "));
if (!strcmp_P(argv[1].str, PSTR("+"))) {
frame_type = PLUS_FRAME;
}else if(argv[1].i == 2){
Serial.printf_P(PSTR("X "));
} else if (!strcmp_P(argv[1].str, PSTR("x"))) {
frame_type = X_FRAME;
}else if(argv[1].i == 3){
Serial.printf_P(PSTR("Tri "));
} else if (!strcmp_P(argv[1].str, PSTR("tri"))) {
frame_type = TRI_FRAME;
} else {
Serial.printf_P(PSTR("\nOptions:[+, x, tri]\n"));
report_frame();
return 0;
}
Serial.printf_P(PSTR("frame\n\n"));
save_EEPROM_frame();
save_EEPROM_frame();
report_frame();
return 0;
}
static int8_t
setup_current(uint8_t argc, const Menu::arg *argv)
{
if (!strcmp_P(argv[1].str, PSTR("on"))) {
current_enabled = true;
save_EEPROM_mag();
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
current_enabled = false;
save_EEPROM_mag();
} else if(argv[1].i > 10){
milliamp_hours = argv[1].i;
} else {
Serial.printf_P(PSTR("\nOptions:[on, off, mAh]\n"));
report_current();
return 0;
}
save_EEPROM_current();
report_current();
return 0;
}
static int8_t
setup_mag_offset(uint8_t argc, const Menu::arg *argv)
@ -764,18 +531,352 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
// set offsets to account for surrounding interference
compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);
print_done();
report_compass();
break;
}
}
}
}
/***************************************************************************/
// CLI utilities
/***************************************************************************/
void print_PID(PID * pid)
void default_waypoint_info()
{
wp_radius = 4; //TODO: Replace this quick fix with a real way to define wp_radius
loiter_radius = 30; //TODO: Replace this quick fix with a real way to define loiter_radius
save_EEPROM_waypoint_info();
}
void
default_nav()
{
// nav control
x_track_gain = XTRACK_GAIN * 100;
x_track_angle = XTRACK_ENTRY_ANGLE * 100;
pitch_max = PITCH_MAX * 100;
save_EEPROM_nav();
}
void
default_alt_hold()
{
alt_to_hold = -1;
save_EEPROM_alt_RTL();
}
void
default_frame()
{
frame_type = PLUS_FRAME;
save_EEPROM_frame();
}
void
default_current()
{
milliamp_hours = 2000;
current_enabled = false;
save_EEPROM_current();
}
void
default_flight_modes()
{
flight_modes[0] = FLIGHT_MODE_1;
flight_modes[1] = FLIGHT_MODE_2;
flight_modes[2] = FLIGHT_MODE_3;
flight_modes[3] = FLIGHT_MODE_4;
flight_modes[4] = FLIGHT_MODE_5;
flight_modes[5] = FLIGHT_MODE_6;
save_EEPROM_flight_modes();
}
void
default_throttle()
{
throttle_min = THROTTLE_MIN;
throttle_max = THROTTLE_MAX;
throttle_cruise = THROTTLE_CRUISE;
throttle_failsafe_enabled = THROTTLE_FAILSAFE;
throttle_failsafe_action = THROTTLE_FAILSAFE_ACTION;
throttle_failsafe_value = THROTTLE_FS_VALUE;
save_EEPROM_throttle();
}
void default_logs()
{
// convenience macro for testing LOG_* and setting LOGBIT_*
#define LOGBIT(_s) (LOG_ ## _s ? LOGBIT_ ## _s : 0)
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
save_EEPROM_logs();
}
void
default_gains()
{
// acro, angular rate
pid_acro_rate_roll.kP(ACRO_RATE_ROLL_P);
pid_acro_rate_roll.kI(ACRO_RATE_ROLL_I);
pid_acro_rate_roll.kD(ACRO_RATE_ROLL_D);
pid_acro_rate_roll.imax(ACRO_RATE_ROLL_IMAX * 100);
pid_acro_rate_pitch.kP(ACRO_RATE_PITCH_P);
pid_acro_rate_pitch.kI(ACRO_RATE_PITCH_I);
pid_acro_rate_pitch.kD(ACRO_RATE_PITCH_D);
pid_acro_rate_pitch.imax(ACRO_RATE_PITCH_IMAX * 100);
pid_acro_rate_yaw.kP(ACRO_RATE_YAW_P);
pid_acro_rate_yaw.kI(ACRO_RATE_YAW_I);
pid_acro_rate_yaw.kD(ACRO_RATE_YAW_D);
pid_acro_rate_yaw.imax(ACRO_RATE_YAW_IMAX * 100);
// stabilize, angle error
pid_stabilize_roll.kP(STABILIZE_ROLL_P);
pid_stabilize_roll.kI(STABILIZE_ROLL_I);
pid_stabilize_roll.kD(STABILIZE_ROLL_D);
pid_stabilize_roll.imax(STABILIZE_ROLL_IMAX * 100);
pid_stabilize_pitch.kP(STABILIZE_PITCH_P);
pid_stabilize_pitch.kI(STABILIZE_PITCH_I);
pid_stabilize_pitch.kD(STABILIZE_PITCH_D);
pid_stabilize_pitch.imax(STABILIZE_PITCH_IMAX * 100);
// YAW hold
pid_yaw.kP(YAW_P);
pid_yaw.kI(YAW_I);
pid_yaw.kD(YAW_D);
pid_yaw.imax(YAW_IMAX * 100);
// custom dampeners
// roll pitch
stabilize_dampener = STABILIZE_DAMPENER;
//yaw
hold_yaw_dampener = HOLD_YAW_DAMPENER;
// navigation
pid_nav_lat.kP(NAV_P);
pid_nav_lat.kI(NAV_I);
pid_nav_lat.kD(NAV_D);
pid_nav_lat.imax(NAV_IMAX * 100);
pid_nav_lon.kP(NAV_P);
pid_nav_lon.kI(NAV_I);
pid_nav_lon.kD(NAV_D);
pid_nav_lon.imax(NAV_IMAX * 100);
pid_baro_throttle.kP(THROTTLE_BARO_P);
pid_baro_throttle.kI(THROTTLE_BARO_I);
pid_baro_throttle.kD(THROTTLE_BARO_D);
pid_baro_throttle.imax(THROTTLE_BARO_IMAX * 100);
pid_sonar_throttle.kP(THROTTLE_SONAR_P);
pid_sonar_throttle.kI(THROTTLE_SONAR_I);
pid_sonar_throttle.kD(THROTTLE_SONAR_D);
pid_sonar_throttle.imax(THROTTLE_SONAR_IMAX * 100);
save_EEPROM_PID();
}
/***************************************************************************/
// CLI utilities
/***************************************************************************/
void report_current()
{
print_blanks(2);
read_EEPROM_current();
Serial.printf_P(PSTR("Current Sensor\n"));
print_divider();
print_enabled(current_enabled);
Serial.printf_P(PSTR("mah: %d"),milliamp_hours);
print_blanks(2);
}
void report_frame()
{
print_blanks(2);
read_EEPROM_frame();
Serial.printf_P(PSTR("Frame\n"));
print_divider();
if(frame_type == X_FRAME)
Serial.printf_P(PSTR("X "));
else if(frame_type == PLUS_FRAME)
Serial.printf_P(PSTR("Plus "));
else if(frame_type == TRI_FRAME)
Serial.printf_P(PSTR("TRI "));
Serial.printf_P(PSTR("frame (%d)"), (int)frame_type);
print_blanks(2);
}
void report_radio()
{
print_blanks(2);
Serial.printf_P(PSTR("Radio\n"));
print_divider();
// radio
read_EEPROM_radio();
print_radio_values();
print_blanks(2);
}
void report_gains()
{
print_blanks(2);
Serial.printf_P(PSTR("Gains\n"));
print_divider();
read_EEPROM_PID();
// Acro
Serial.printf_P(PSTR("Acro:\nroll:\n"));
print_PID(&pid_acro_rate_roll);
Serial.printf_P(PSTR("pitch:\n"));
print_PID(&pid_acro_rate_pitch);
Serial.printf_P(PSTR("yaw:\n"));
print_PID(&pid_acro_rate_yaw);
// Stabilize
Serial.printf_P(PSTR("\nStabilize:\nroll:\n"));
print_PID(&pid_stabilize_roll);
Serial.printf_P(PSTR("pitch:\n"));
print_PID(&pid_stabilize_pitch);
Serial.printf_P(PSTR("yaw:\n"));
print_PID(&pid_yaw);
Serial.printf_P(PSTR("Stabilize dampener: %4.3f\n"), stabilize_dampener);
Serial.printf_P(PSTR("Yaw Dampener: %4.3f\n\n"), hold_yaw_dampener);
// Nav
Serial.printf_P(PSTR("Nav:\nlat:\n"));
print_PID(&pid_nav_lat);
Serial.printf_P(PSTR("long:\n"));
print_PID(&pid_nav_lon);
Serial.printf_P(PSTR("baro throttle:\n"));
print_PID(&pid_baro_throttle);
Serial.printf_P(PSTR("sonar throttle:\n"));
print_PID(&pid_sonar_throttle);
print_blanks(2);
}
void report_xtrack()
{
print_blanks(2);
Serial.printf_P(PSTR("Crosstrack\n"));
print_divider();
// radio
read_EEPROM_nav();
Serial.printf_P(PSTR("XTRACK: %4.2f\n"
"XTRACK angle: %d\n"
"PITCH_MAX: %d"),
x_track_gain,
x_track_angle,
pitch_max);
print_blanks(2);
}
void report_throttle()
{
print_blanks(2);
Serial.printf_P(PSTR("Throttle\n"));
print_divider();
read_EEPROM_throttle();
Serial.printf_P(PSTR("min: %d\n"
"max: %d\n"
"cruise: %d\n"
"failsafe_enabled: %d\n"
"failsafe_value: %d"),
throttle_min,
throttle_max,
throttle_cruise,
throttle_failsafe_enabled,
throttle_failsafe_value);
print_blanks(2);
}
void report_imu()
{
print_blanks(2);
Serial.printf_P(PSTR("IMU\n"));
print_divider();
imu.print_gyro_offsets();
imu.print_accel_offsets();
print_blanks(2);
}
void report_compass()
{
print_blanks(2);
Serial.printf_P(PSTR("Compass\n"));
print_divider();
read_EEPROM_compass();
read_EEPROM_compass_declination();
read_EEPROM_compass_offset();
print_enabled(compass_enabled);
// mag declination
Serial.printf_P(PSTR("Mag Delination: %4.4f\n"),
mag_declination);
// mag offsets
Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f"),
mag_offset_x,
mag_offset_y,
mag_offset_z);
print_blanks(2);
}
void report_flight_modes()
{
print_blanks(2);
Serial.printf_P(PSTR("Flight modes\n"));
print_divider();
read_EEPROM_flight_modes();
for(int i = 0; i < 6; i++ ){
print_switch(i, flight_modes[i]);
}
print_blanks(2);
}
/***************************************************************************/
// CLI utilities
/***************************************************************************/
void
print_PID(PID * pid)
{
Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"), pid->kP(), pid->kI(), pid->kD(), (long)pid->imax());
}
@ -854,3 +955,16 @@ void zero_eeprom(void)
}
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"));
}

2
ArduCopterMega/system.pde
View File

@ -85,7 +85,7 @@ void init_ardupilot()
#endif
Serial.printf_P(PSTR("\n\n"
"Init ArduPilotMega 1.0.3 Public Alpha\n\n"
"Init ArduCopterMega 1.0.0 Public Alpha\n\n"
#if TELEMETRY_PORT == 3
"Telemetry is on the xbee port\n"
#endif