mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
updated to AP_Var
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1680 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
02b0e92d5b
commit
6f32b9e408
@ -1,7 +1,7 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
/*
|
||||
ArduCopterMega Version 0.1 Experimental
|
||||
ArduCopterMega Version 0.1.3 Experimental
|
||||
Authors: Jason Short
|
||||
Based on code and ideas from the Arducopter team: Jose Julio, Randy Mackay, Jani Hirvinen
|
||||
Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier
|
||||
@ -216,8 +216,7 @@ byte command_may_index; // current command memory location
|
||||
byte command_must_ID; // current command ID
|
||||
byte command_may_ID; // current command ID
|
||||
|
||||
float altitude_gain; // in nav
|
||||
float distance_gain; // in nav
|
||||
//float altitude_gain; // in nav
|
||||
|
||||
float cos_roll_x;
|
||||
float sin_roll_y;
|
||||
@ -281,11 +280,9 @@ int milliamp_hours;
|
||||
|
||||
// Barometer Sensor variables
|
||||
// --------------------------
|
||||
//int baro_offset; // used to correct drift of absolute pressue sensor
|
||||
unsigned long abs_pressure;
|
||||
unsigned long abs_pressure_ground;
|
||||
unsigned long ground_pressure;
|
||||
int ground_temperature;
|
||||
//int temp_unfilt;
|
||||
|
||||
byte altitude_sensor = BARO; // used to know whic sensor is active, BARO or SONAR
|
||||
|
||||
|
||||
@ -5,7 +5,7 @@
|
||||
|
||||
// Macro functions
|
||||
// ---------------
|
||||
void read_EEPROM_startup(void)
|
||||
/*void read_EEPROM_startup(void)
|
||||
{
|
||||
read_EEPROM_PID();
|
||||
read_EEPROM_frame();
|
||||
@ -13,27 +13,17 @@ void read_EEPROM_startup(void)
|
||||
read_EEPROM_logs();
|
||||
read_EEPROM_flight_modes();
|
||||
read_EEPROM_waypoint_info();
|
||||
imu.load_gyro_eeprom();
|
||||
imu.load_accel_eeprom();
|
||||
//imu.load_gyro_eeprom();
|
||||
//imu.load_accel_eeprom();
|
||||
read_EEPROM_alt_RTL();
|
||||
read_EEPROM_current();
|
||||
read_EEPROM_nav();
|
||||
// magnatometer
|
||||
read_EEPROM_compass();
|
||||
read_EEPROM_compass_declination();
|
||||
//read_EEPROM_compass_declination();
|
||||
read_EEPROM_compass_offset();
|
||||
}
|
||||
|
||||
void read_EEPROM_airstart_critical(void)
|
||||
{
|
||||
// Read the home location
|
||||
//-----------------------
|
||||
home = get_wp_with_index(0);
|
||||
|
||||
// Read pressure sensor values
|
||||
//----------------------------
|
||||
read_EEPROM_pressure();
|
||||
}
|
||||
*/
|
||||
|
||||
void save_EEPROM_groundstart(void)
|
||||
{
|
||||
@ -56,56 +46,45 @@ void save_EEPROM_groundstart(void)
|
||||
|
||||
void save_EEPROM_alt_RTL(void)
|
||||
{
|
||||
eeprom_write_dword((uint32_t *)EE_ALT_HOLD_HOME, alt_to_hold);
|
||||
g.RTL_altitude.save();
|
||||
|
||||
}
|
||||
|
||||
void read_EEPROM_alt_RTL(void)
|
||||
{
|
||||
alt_to_hold = eeprom_read_dword((const uint32_t *) EE_ALT_HOLD_HOME);
|
||||
g.RTL_altitude.load();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
void read_EEPROM_waypoint_info(void)
|
||||
{
|
||||
g.waypoint_total = eeprom_read_byte((uint8_t *) EE_WP_TOTAL);
|
||||
wp_radius = eeprom_read_byte((uint8_t *) EE_WP_RADIUS);
|
||||
loiter_radius = eeprom_read_byte((uint8_t *) EE_LOITER_RADIUS);
|
||||
g.waypoint_total.load();
|
||||
g.waypoint_radius.load();
|
||||
g.loiter_radius.load();
|
||||
}
|
||||
|
||||
void save_EEPROM_waypoint_info(void)
|
||||
{
|
||||
eeprom_write_byte((uint8_t *) EE_WP_TOTAL, g.waypoint_total);
|
||||
eeprom_write_byte((uint8_t *) EE_WP_RADIUS, wp_radius);
|
||||
eeprom_write_byte((uint8_t *) EE_LOITER_RADIUS, loiter_radius);
|
||||
g.waypoint_total.save();
|
||||
g.waypoint_radius.save();
|
||||
g.loiter_radius.save();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
void read_EEPROM_nav(void)
|
||||
{
|
||||
// for nav estimation
|
||||
distance_gain = read_EE_compressed_float(EE_DISTANCE_GAIN, 4);
|
||||
altitude_gain = read_EE_compressed_float(EE_ALTITUDE_GAIN, 4);
|
||||
|
||||
// stored as degree * 100
|
||||
g.crosstrack_gain = read_EE_compressed_float(EE_XTRACK_GAIN, 4);
|
||||
g.crosstrack_entry_angle = eeprom_read_word((uint16_t *) EE_XTRACK_ANGLE) * 100;
|
||||
pitch_max = eeprom_read_word((uint16_t *) EE_PITCH_MAX); // stored as degress * 100
|
||||
g.crosstrack_gain.load();
|
||||
g.crosstrack_entry_angle.load();
|
||||
g.pitch_max.load();
|
||||
}
|
||||
|
||||
void save_EEPROM_nav(void)
|
||||
{
|
||||
// for nav estimation
|
||||
write_EE_compressed_float(altitude_gain, EE_ALTITUDE_GAIN, 4);
|
||||
write_EE_compressed_float(distance_gain, EE_DISTANCE_GAIN, 4);
|
||||
write_EE_compressed_float(crosstrack_gain, EE_XTRACK_GAIN, 4);
|
||||
|
||||
// stored as degrees
|
||||
eeprom_write_word((uint16_t *) EE_XTRACK_ANGLE, g.crosstrack_entry_angle / 100);
|
||||
|
||||
// stored as degrees
|
||||
eeprom_write_word((uint16_t *) EE_PITCH_MAX, pitch_max);
|
||||
g.crosstrack_gain.save();
|
||||
g.crosstrack_entry_angle.save();
|
||||
g.pitch_max.save();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
@ -126,10 +105,10 @@ void read_EEPROM_PID(void)
|
||||
g.pid_sonar_throttle.load_gains();
|
||||
|
||||
// roll pitch
|
||||
stabilize_dampener = read_EE_compressed_float(EE_STAB_DAMPENER, 4);
|
||||
g.stabilize_dampener.load();
|
||||
|
||||
// yaw
|
||||
hold_yaw_dampener = read_EE_compressed_float(EE_HOLD_YAW_DAMPENER, 4);
|
||||
g.hold_yaw_dampener.load();
|
||||
init_pids();
|
||||
}
|
||||
|
||||
@ -149,61 +128,47 @@ void save_EEPROM_PID(void)
|
||||
g.pid_sonar_throttle.save_gains();
|
||||
|
||||
// roll pitch
|
||||
write_EE_compressed_float(stabilize_dampener, EE_STAB_DAMPENER, 4);
|
||||
|
||||
g.stabilize_dampener.save();
|
||||
// yaw
|
||||
write_EE_compressed_float(hold_yaw_dampener, EE_HOLD_YAW_DAMPENER, 4);
|
||||
g.hold_yaw_dampener.save();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
void save_EEPROM_frame(void)
|
||||
{
|
||||
eeprom_write_byte((uint8_t *)EE_FRAME, frame_type);
|
||||
g.frame_type.save();
|
||||
}
|
||||
|
||||
void read_EEPROM_frame(void)
|
||||
{
|
||||
frame_type = eeprom_read_byte((uint8_t *) EE_FRAME);
|
||||
g.frame_type.load();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
void save_EEPROM_g.(void)
|
||||
void save_EEPROM_throttle_cruise (void)
|
||||
{
|
||||
eeprom_write_word((uint16_t *)EE_THROTTLE_CRUISE, g.);
|
||||
g.throttle_cruise.save();
|
||||
}
|
||||
|
||||
void read_EEPROM_g.(void)
|
||||
void read_EEPROM_throttle_cruise(void)
|
||||
{
|
||||
g.throttle_cruise = eeprom_read_word((uint16_t *) EE_THROTTLE_CRUISE);
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
void save_EEPROM_mag_declination(void)
|
||||
{
|
||||
write_EE_compressed_float(mag_declination, EE_MAG_DECLINATION, 1);
|
||||
}
|
||||
|
||||
void read_EEPROM_compass_declination(void)
|
||||
{
|
||||
mag_declination = read_EE_compressed_float(EE_MAG_DECLINATION, 1);
|
||||
g.throttle_cruise.load();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
void save_EEPROM_current(void)
|
||||
{
|
||||
eeprom_write_byte((uint8_t *) EE_CURRENT_SENSOR, current_enabled);
|
||||
eeprom_write_word((uint16_t *) EE_CURRENT_MAH, milliamp_hours);
|
||||
|
||||
g.current_enabled.save();
|
||||
g.milliamp_hours.save();
|
||||
}
|
||||
|
||||
void read_EEPROM_current(void)
|
||||
{
|
||||
current_enabled = eeprom_read_byte((uint8_t *) EE_CURRENT_SENSOR);
|
||||
milliamp_hours = eeprom_read_word((uint16_t *) EE_CURRENT_MAH);
|
||||
g.current_enabled.load();
|
||||
g.milliamp_hours.load();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
@ -226,40 +191,42 @@ void read_EEPROM_compass_offset(void)
|
||||
|
||||
void read_EEPROM_compass(void)
|
||||
{
|
||||
compass_enabled = eeprom_read_byte((uint8_t *) EE_COMPASS);
|
||||
g.compass_enabled.load();
|
||||
}
|
||||
|
||||
void save_EEPROM_mag(void)
|
||||
{
|
||||
eeprom_write_byte((uint8_t *) EE_COMPASS, compass_enabled);
|
||||
g.compass_enabled.save();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
void save_command_index(void)
|
||||
{
|
||||
eeprom_write_byte((uint8_t *) EE_WP_INDEX, command_must_index);
|
||||
g.command_must_index.save();
|
||||
}
|
||||
|
||||
void read_command_index(void)
|
||||
{
|
||||
g.waypoint_index = command_must_index = eeprom_read_byte((uint8_t *) EE_WP_INDEX);
|
||||
g.command_must_index.load();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
void save_EEPROM_pressure(void)
|
||||
{
|
||||
eeprom_write_dword((uint32_t *)EE_ABS_PRESS_GND, abs_pressure_ground);
|
||||
eeprom_write_word((uint16_t *)EE_GND_TEMP, ground_temperature);
|
||||
g.ground_pressure.save();
|
||||
g.ground_temperature.save();
|
||||
|
||||
}
|
||||
|
||||
void read_EEPROM_pressure(void)
|
||||
{
|
||||
abs_pressure_ground = eeprom_read_dword((uint32_t *) EE_ABS_PRESS_GND);
|
||||
// Better than zero for an air start value
|
||||
abs_pressure = abs_pressure_ground;
|
||||
ground_temperature = eeprom_read_word((uint16_t *) EE_GND_TEMP);
|
||||
g.ground_pressure.load();
|
||||
g.ground_temperature.load();
|
||||
|
||||
// to prime the filter
|
||||
abs_pressure = g.ground_pressure;
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
@ -292,52 +259,51 @@ void save_EEPROM_radio(void)
|
||||
// configs are the basics
|
||||
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);
|
||||
read_EEPROM_g.();
|
||||
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);
|
||||
g.throttle_min.load();
|
||||
g.throttle_max.load();
|
||||
g.throttle_cruise.load();
|
||||
g.throttle_failsafe_enabled.load();
|
||||
g.throttle_failsafe_action.load();
|
||||
g.throttle_failsafe_value.load();
|
||||
}
|
||||
|
||||
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);
|
||||
save_EEPROM_g.();
|
||||
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);
|
||||
g.throttle_min.load();
|
||||
g.throttle_max.load();
|
||||
g.throttle_cruise.save();
|
||||
g.throttle_failsafe_enabled.load();
|
||||
g.throttle_failsafe_action.load();
|
||||
g.throttle_failsafe_value.load();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
void read_EEPROM_logs(void)
|
||||
{
|
||||
g.log_bitmask = eeprom_read_word((uint16_t *) EE_LOG_BITMASK);
|
||||
g.log_bitmask.load();
|
||||
}
|
||||
|
||||
void save_EEPROM_logs(void)
|
||||
{
|
||||
eeprom_write_word((uint16_t *) EE_LOG_BITMASK, log_bitmask);
|
||||
g.log_bitmask.save();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
void read_EEPROM_flight_modes(void)
|
||||
{
|
||||
// Read Radio min/max settings
|
||||
eeprom_read_block((void*)&flight_modes, (const void*)EE_FLIGHT_MODES, sizeof(flight_modes));
|
||||
g.flight_modes.load();
|
||||
}
|
||||
|
||||
void save_EEPROM_flight_modes(void)
|
||||
{
|
||||
// Save Radio min/max settings
|
||||
eeprom_write_block((const void *)&flight_modes, (void *)EE_FLIGHT_MODES, sizeof(flight_modes));
|
||||
g.flight_modes.save();
|
||||
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
/*
|
||||
float
|
||||
read_EE_float(int address)
|
||||
{
|
||||
@ -362,9 +328,9 @@ void write_EE_float(float value, int address)
|
||||
for (int i = 0; i < 4; i++)
|
||||
eeprom_write_byte((uint8_t *) (address + i), _floatIn.bytes[i]);
|
||||
}
|
||||
|
||||
*/
|
||||
/********************************************************************************/
|
||||
|
||||
/*
|
||||
float
|
||||
read_EE_compressed_float(int address, byte places)
|
||||
{
|
||||
@ -380,3 +346,4 @@ void write_EE_compressed_float(float value, int address, byte places)
|
||||
int temp = value * scale;
|
||||
eeprom_write_word((uint16_t *) address, temp);
|
||||
}
|
||||
*/
|
||||
@ -55,11 +55,10 @@ public:
|
||||
//
|
||||
k_param_IMU_calibration = 140,
|
||||
k_param_ground_temperature,
|
||||
k_param_ground_altitude,
|
||||
k_param_ground_pressure,
|
||||
k_param_current,
|
||||
k_param_milliamp_hours,
|
||||
k_param_compass,
|
||||
k_param_mag_declination,
|
||||
|
||||
//
|
||||
// 160: Navigation parameters
|
||||
@ -97,6 +96,7 @@ public:
|
||||
k_param_waypoint_mode = 220,
|
||||
k_param_waypoint_total,
|
||||
k_param_waypoint_index,
|
||||
k_param_command_must_index,
|
||||
k_param_waypoint_radius,
|
||||
k_param_loiter_radius,
|
||||
|
||||
@ -138,6 +138,7 @@ public:
|
||||
AP_Int8 waypoint_mode;
|
||||
AP_Int8 waypoint_total;
|
||||
AP_Int8 waypoint_index;
|
||||
AP_Int8 command_must_index;
|
||||
AP_Int8 waypoint_radius;
|
||||
AP_Int8 loiter_radius;
|
||||
|
||||
@ -168,14 +169,13 @@ public:
|
||||
//
|
||||
AP_Int16 log_bitmask;
|
||||
AP_Int16 ground_temperature;
|
||||
AP_Int16 ground_altitude;
|
||||
AP_Int16 ground_pressure;
|
||||
AP_Int16 RTL_altitude;
|
||||
AP_Int8 frame_type;
|
||||
|
||||
AP_Int8 current_enabled;
|
||||
AP_Int16 milliamp_hours;
|
||||
AP_Int8 compass_enabled;
|
||||
AP_Float mag_declination;
|
||||
|
||||
|
||||
// RC channels
|
||||
@ -218,21 +218,22 @@ public:
|
||||
frame_type (FRAME_CONFIG, k_param_frame_type, PSTR("FRAME_CONFIG")),
|
||||
|
||||
current_enabled (DISABLED, k_param_current, PSTR("CURRENT_ENABLE")),
|
||||
milliamp_hours (2100, k_param_milliamp_hours, PSTR("MAH")),
|
||||
compass_enabled (DISABLED, k_param_compass, PSTR("COMPASS_ENABLE")),
|
||||
mag_declination (0, k_param_mag_declination, PSTR("MAG_DEC")),
|
||||
|
||||
waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")),
|
||||
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),
|
||||
waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")),
|
||||
command_must_index (0, k_param_command_must_index, PSTR("WP_MUST_INDEX")),
|
||||
waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")),
|
||||
loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("LOITER_RADIUS")),
|
||||
|
||||
throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")),
|
||||
throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")),
|
||||
throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")),
|
||||
throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")),
|
||||
throttle_failsafe_enabled (THROTTLE_FAILSAFE, k_param_throttle_failsafe_enabled, PSTR("THR_FAILSAFE")),
|
||||
throttle_failsafe_action (THROTTLE_FAILSAFE_ACTION, k_param_throttle_failsafe_action, PSTR("THR_FS_ACTION")),
|
||||
throttle_failsafe_value (THROTTLE_FS_VALUE, k_param_throttle_failsafe_value, PSTR("THR_FS_VALUE")),
|
||||
throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")),
|
||||
throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")),
|
||||
|
||||
flight_mode_channel (FLIGHT_MODE_CHANNEL, k_param_flight_mode_channel, PSTR("FLIGHT_MODE_CH")),
|
||||
flight_modes (k_param_flight_modes, PSTR("FLIGHT_MODES")),
|
||||
@ -241,7 +242,6 @@ public:
|
||||
|
||||
log_bitmask (0, k_param_log_bitmask, PSTR("LOG_BITMASK")),
|
||||
ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")),
|
||||
ground_altitude (0, k_param_ground_altitude, PSTR("GND_ALT_CM")),
|
||||
ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")),
|
||||
RTL_altitude (ALT_HOLD_HOME_CM, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),
|
||||
|
||||
|
||||
@ -155,7 +155,7 @@ void
|
||||
set_next_WP(struct Location *wp)
|
||||
{
|
||||
Serial.printf_P(PSTR("set_next_WP, wp_index %d\n"), (int)g.waypoint_index);
|
||||
send_message(MSG_COMMAND, g.waypoint_index);
|
||||
send_message(MSG_COMMAND_LIST, g.waypoint_index);
|
||||
|
||||
// copy the current WP into the OldWP slot
|
||||
// ---------------------------------------
|
||||
|
||||
@ -8,12 +8,12 @@ void update_commands(void)
|
||||
if((home_is_set == false) || (control_mode != AUTO)){
|
||||
return; // don't do commands
|
||||
}
|
||||
|
||||
|
||||
if(control_mode == AUTO){
|
||||
load_next_command();
|
||||
process_next_command();
|
||||
}
|
||||
|
||||
|
||||
//verify_must();
|
||||
//verify_may();
|
||||
}
|
||||
@ -26,32 +26,32 @@ void load_next_command()
|
||||
if(next_command.id == CMD_BLANK){
|
||||
next_command = get_wp_with_index(g.waypoint_index + 1);
|
||||
if(next_command.id != CMD_BLANK){
|
||||
//Serial.print("MSG fetch found new cmd from list at index: ");
|
||||
//Serial.println((g.waypoint_index + 1),DEC);
|
||||
//Serial.print("MSG cmd id: ");
|
||||
//Serial.println(next_command.id,DEC);
|
||||
//SendDebug("MSG <load_next_command> fetch found new cmd from list at index ");
|
||||
//SendDebug((g.waypoint_index + 1),DEC);
|
||||
//SendDebug(" with cmd id ");
|
||||
//SendDebugln(next_command.id,DEC);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// If the preload failed, return or just Loiter
|
||||
// generate a dynamic command for RTL
|
||||
// --------------------------------------------
|
||||
if(next_command.id == CMD_BLANK){
|
||||
// we are out of commands!
|
||||
//send_message(SEVERITY_LOW,"out of commands!");
|
||||
//Serial.print("MSG out of commands, wp_index: ");
|
||||
//Serial.println(g.waypoint_index,DEC);
|
||||
gcs.send_text(SEVERITY_LOW,"out of commands!");
|
||||
//SendDebug("MSG <load_next_command> out of commands, g.waypoint_index: ");
|
||||
//SendDebugln(g.waypoint_index,DEC);
|
||||
|
||||
|
||||
|
||||
switch (control_mode){
|
||||
case LAND:
|
||||
// don't get a new command
|
||||
break;
|
||||
|
||||
|
||||
default:
|
||||
next_command = get_LOITER_home_wp();
|
||||
//Serial.print("MSG Preload RTL cmd id: ");
|
||||
//Serial.println(next_command.id,DEC);
|
||||
//SendDebug("MSG <load_next_command> Preload RTL cmd id: ");
|
||||
//SendDebugln(next_command.id,DEC);
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -62,25 +62,25 @@ void process_next_command()
|
||||
// these are waypoint/Must commands
|
||||
// ---------------------------------
|
||||
if (command_must_index == 0){ // no current command loaded
|
||||
if (next_command.id >= 0x10 && next_command.id <= 0x1F ){
|
||||
if (next_command.id < MAV_CMD_NAV_LAST ){
|
||||
increment_WP_index();
|
||||
save_command_index(); // to Recover from in air Restart
|
||||
//save_command_index(); // to Recover from in air Restart
|
||||
command_must_index = g.waypoint_index;
|
||||
|
||||
//Serial.print("MSG new command_must_id ");
|
||||
//Serial.print(next_command.id,DEC);
|
||||
//Serial.print(" index:");
|
||||
//Serial.println(command_must_index,DEC);
|
||||
if (g.log_bitmask & MASK_LOG_CMD)
|
||||
|
||||
//SendDebug("MSG <process_next_command> new command_must_id ");
|
||||
//SendDebug(next_command.id,DEC);
|
||||
//SendDebug(" index:");
|
||||
//SendDebugln(command_must_index,DEC);
|
||||
if (g.log_bitmask & MASK_LOG_CMD)
|
||||
Log_Write_Cmd(g.waypoint_index, &next_command);
|
||||
process_must();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// these are May commands
|
||||
// ----------------------
|
||||
if (command_may_index == 0){
|
||||
if (next_command.id >= 0x20 && next_command.id <= 0x2F ){
|
||||
if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){
|
||||
increment_WP_index();// this command is from the command list in EEPROM
|
||||
command_may_index = g.waypoint_index;
|
||||
//Serial.print("new command_may_index ");
|
||||
@ -90,50 +90,50 @@ void process_next_command()
|
||||
process_may();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// these are do it now commands
|
||||
// ---------------------------
|
||||
if (next_command.id >= 0x30){
|
||||
increment_WP_index();// this command is from the command list in EEPROM
|
||||
if (next_command.id > MAV_CMD_CONDITION_LAST){
|
||||
increment_WP_index();// this command is from the command list in EEPROM
|
||||
if (g.log_bitmask & MASK_LOG_CMD)
|
||||
Log_Write_Cmd(g.waypoint_index, &next_command);
|
||||
Log_Write_Cmd(g.waypoint_index, &next_command);
|
||||
process_now();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
These functions implement the waypoint commands.
|
||||
*/
|
||||
void process_must()
|
||||
{
|
||||
//Serial.print("process must index: ");
|
||||
//Serial.println(command_must_index,DEC);
|
||||
//SendDebug("process must index: ");
|
||||
//SendDebugln(command_must_index,DEC);
|
||||
|
||||
gcs.send_text(SEVERITY_LOW,"New cmd: <process_must>");
|
||||
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
|
||||
|
||||
send_message(SEVERITY_LOW,"New cmd: ");
|
||||
send_message(MSG_COMMAND, g.waypoint_index);
|
||||
|
||||
// clear May indexes
|
||||
command_may_index = 0;
|
||||
command_may_ID = 0;
|
||||
|
||||
command_must_ID = next_command.id;
|
||||
|
||||
// invalidate command so a new one is loaded
|
||||
// -----------------------------------------
|
||||
next_command.id = 0;
|
||||
|
||||
// reset navigation integrators
|
||||
// -------------------------
|
||||
reset_I();
|
||||
|
||||
// loads the waypoint into Next_WP struct
|
||||
// --------------------------------------
|
||||
set_next_WP(&next_command);
|
||||
|
||||
|
||||
// invalidate command so a new one is loaded
|
||||
// -----------------------------------------
|
||||
next_command.id = 0;
|
||||
|
||||
// reset navigation integrators
|
||||
// -------------------------
|
||||
reset_I();
|
||||
|
||||
switch(command_must_ID){
|
||||
|
||||
case CMD_TAKEOFF: // TAKEOFF!
|
||||
|
||||
case MAV_CMD_NAV_TAKEOFF: // TAKEOFF!
|
||||
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
|
||||
takeoff_altitude = (int)next_command.alt;
|
||||
next_WP.lat = current_loc.lat + 10; // so we don't have bad calcs
|
||||
next_WP.lng = current_loc.lng + 10; // so we don't have bad calcs
|
||||
@ -142,10 +142,10 @@ void process_must()
|
||||
//set_next_WP(&next_WP);
|
||||
break;
|
||||
|
||||
case CMD_WAYPOINT: // Navigate to Waypoint
|
||||
case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
|
||||
break;
|
||||
|
||||
case CMD_R_WAYPOINT: // Navigate to Waypoint
|
||||
case MAV_CMD_NAV_R_WAYPOINT: // Navigate to Waypoint
|
||||
next_command.lat += home.lat; // offset from home location
|
||||
next_command.lng += home.lng; // offset from home location
|
||||
|
||||
@ -153,7 +153,7 @@ void process_must()
|
||||
set_next_WP(&next_command);
|
||||
break;
|
||||
|
||||
case CMD_LAND: // LAND to Waypoint
|
||||
case MAV_CMD_NAV_LAND: // LAND to Waypoint
|
||||
velocity_land = 1000;
|
||||
next_WP.lat = current_loc.lat;
|
||||
next_WP.lng = current_loc.lng;
|
||||
@ -161,53 +161,53 @@ void process_must()
|
||||
land_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
|
||||
break;
|
||||
|
||||
case CMD_ALTITUDE: // Loiter indefinitely
|
||||
/*
|
||||
case MAV_CMD_ALTITUDE: //
|
||||
next_WP.lat = current_loc.lat + 10; // so we don't have bad calcs
|
||||
next_WP.lng = current_loc.lng + 10; // so we don't have bad calcs
|
||||
break;
|
||||
*/
|
||||
|
||||
case CMD_LOITER: // Loiter indefinitely
|
||||
case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
|
||||
break;
|
||||
|
||||
case CMD_LOITER_N_TURNS: // Loiter N Times
|
||||
case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times
|
||||
break;
|
||||
|
||||
case CMD_LOITER_TIME:
|
||||
case MAV_CMD_NAV_LOITER_TIME:
|
||||
break;
|
||||
|
||||
case CMD_RTL:
|
||||
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
||||
return_to_launch();
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void process_may()
|
||||
{
|
||||
//Serial.print("process_may cmd# ");
|
||||
//Serial.println(g.waypoint_index,DEC);
|
||||
//Serial.println(g.waypoint_index,DEC);
|
||||
command_may_ID = next_command.id;
|
||||
|
||||
// invalidate command so a new one is loaded
|
||||
// -----------------------------------------
|
||||
next_command.id = 0;
|
||||
|
||||
send_message(SEVERITY_LOW,"New cmd: ");
|
||||
send_message(MSG_COMMAND, g.waypoint_index);
|
||||
|
||||
|
||||
gcs.send_text(SEVERITY_LOW,"<process_may> New may command loaded:");
|
||||
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
|
||||
// do the command
|
||||
// --------------
|
||||
switch(command_may_ID){
|
||||
|
||||
case CMD_DELAY: // Navigate to Waypoint
|
||||
|
||||
case MAV_CMD_CONDITION_DELAY: // Navigate to Waypoint
|
||||
delay_start = millis();
|
||||
delay_timeout = next_command.lat;
|
||||
break;
|
||||
|
||||
case CMD_LAND_OPTIONS: // Land this puppy
|
||||
break;
|
||||
//case MAV_CMD_NAV_LAND_OPTIONS: // Land this puppy
|
||||
// break;
|
||||
|
||||
case CMD_ANGLE:
|
||||
case MAV_CMD_CONDITION_ANGLE:
|
||||
// p1: bearing
|
||||
// alt: speed
|
||||
// lat: direction (-1,1),
|
||||
@ -281,38 +281,16 @@ void process_now()
|
||||
// invalidate command so a new one is loaded
|
||||
// -----------------------------------------
|
||||
next_command.id = 0;
|
||||
|
||||
send_message(SEVERITY_LOW, "New cmd: ");
|
||||
send_message(MSG_COMMAND, g.waypoint_index);
|
||||
|
||||
|
||||
gcs.send_text(SEVERITY_LOW, "<process_now> New now command loaded: ");
|
||||
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
|
||||
|
||||
// do the command
|
||||
// --------------
|
||||
switch(id){
|
||||
|
||||
case CMD_RESET_INDEX:
|
||||
init_commands();
|
||||
break;
|
||||
|
||||
case CMD_GETVAR_INDEX:
|
||||
//
|
||||
break;
|
||||
|
||||
case CMD_SENDVAR_INDEX:
|
||||
//
|
||||
break;
|
||||
|
||||
case CMD_TELEMETRY:
|
||||
//
|
||||
break;
|
||||
|
||||
//case CMD_AIRSPEED_CRUISE:
|
||||
//airspeed_cruise = next_command.p1 * 100;
|
||||
// todo save to EEPROM
|
||||
//break;
|
||||
|
||||
/*
|
||||
case CMD_THROTTLE_CRUISE:
|
||||
g.throttle_cruise = next_command.p1;
|
||||
// todo save to EEPROM
|
||||
break;
|
||||
|
||||
case CMD_RESET_HOME:
|
||||
@ -356,29 +334,33 @@ void process_now()
|
||||
// radio_min[next_command.p1] = next_command.alt;
|
||||
// save_EEPROM_radio_minmax();
|
||||
// break;
|
||||
|
||||
case CMD_REPEAT:
|
||||
*/
|
||||
|
||||
case MAV_CMD_DO_SET_HOME:
|
||||
init_home();
|
||||
break;
|
||||
|
||||
case MAV_CMD_DO_REPEAT_SERVO:
|
||||
new_event(&next_command);
|
||||
break;
|
||||
|
||||
case CMD_SERVO:
|
||||
case MAV_CMD_DO_SET_SERVO:
|
||||
//Serial.print("MAV_CMD_DO_SET_SERVO ");
|
||||
//Serial.print(next_command.p1,DEC);
|
||||
//Serial.print(" PWM: ");
|
||||
//Serial.println(next_command.alt,DEC);
|
||||
APM_RC.OutputCh(next_command.p1, next_command.alt);
|
||||
break;
|
||||
|
||||
case CMD_INDEX:
|
||||
command_must_index = 0;
|
||||
command_may_index = 0;
|
||||
g.waypoint_index = next_command.p1 - 1;
|
||||
break;
|
||||
|
||||
case CMD_RELAY:
|
||||
if(next_command.p1 = 0){
|
||||
relay_A();
|
||||
case MAV_CMD_DO_SET_RELAY:
|
||||
if (next_command.p1 == 0) {
|
||||
relay_on();
|
||||
} else if (next_command.p1 == 1) {
|
||||
relay_off();
|
||||
}else{
|
||||
relay_B();
|
||||
relay_toggle();
|
||||
}
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -386,24 +368,24 @@ void process_now()
|
||||
// ---------------
|
||||
void verify_must()
|
||||
{
|
||||
float power;
|
||||
|
||||
switch(command_must_ID) {
|
||||
|
||||
case CMD_ALTITUDE:
|
||||
/*case MAV_CMD_ALTITUDE:
|
||||
if (abs(next_WP.alt - current_loc.alt) < 100){
|
||||
command_must_index = 0;
|
||||
}
|
||||
break;
|
||||
*/
|
||||
|
||||
case CMD_TAKEOFF: // Takeoff!
|
||||
case MAV_CMD_NAV_TAKEOFF: // Takeoff!
|
||||
if (current_loc.alt > (next_WP.alt -100)){
|
||||
command_must_index = 0;
|
||||
takeoff_complete = true;
|
||||
}
|
||||
break;
|
||||
|
||||
case CMD_LAND:
|
||||
case MAV_CMD_NAV_LAND:
|
||||
// 10 - 9 = 1
|
||||
velocity_land = ((old_alt - current_loc.alt) *.05) + (velocity_land * .95);
|
||||
old_alt = current_loc.alt;
|
||||
@ -411,12 +393,19 @@ void verify_must()
|
||||
land_complete = true;
|
||||
command_must_index = 0;
|
||||
}
|
||||
update_crosstrack();
|
||||
|
||||
break;
|
||||
|
||||
case CMD_WAYPOINT: // reach a waypoint
|
||||
if ((wp_distance > 0) && (wp_distance <= waypoint_radius)) {
|
||||
Serial.print("MSG REACHED_WAYPOINT #");
|
||||
Serial.println(command_must_index,DEC);
|
||||
|
||||
case MAV_CMD_NAV_WAYPOINT: // reach a waypoint
|
||||
update_crosstrack();
|
||||
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
|
||||
//SendDebug("MSG <verify_must: MAV_CMD_NAV_WAYPOINT> REACHED_WAYPOINT #");
|
||||
//SendDebugln(command_must_index,DEC);
|
||||
char message[50];
|
||||
sprintf(message,"Reached Waypoint #%i",command_must_index);
|
||||
gcs.send_text(SEVERITY_LOW,message);
|
||||
|
||||
// clear the command queue;
|
||||
command_must_index = 0;
|
||||
}
|
||||
@ -428,23 +417,35 @@ void verify_must()
|
||||
}
|
||||
break;
|
||||
|
||||
case CMD_LOITER_N_TURNS: // LOITER N times
|
||||
case MAV_CMD_NAV_LOITER_TURNS: // LOITER N times
|
||||
break;
|
||||
|
||||
case CMD_LOITER_TIME: // loiter N milliseconds
|
||||
|
||||
case MAV_CMD_NAV_LOITER_TIME: // loiter N milliseconds
|
||||
break;
|
||||
|
||||
case CMD_RTL:
|
||||
if (wp_distance <= waypoint_radius) {
|
||||
//Serial.println("REACHED_HOME");
|
||||
|
||||
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
||||
if (wp_distance <= g.waypoint_radius) {
|
||||
gcs.send_text(SEVERITY_LOW,"Reached home");
|
||||
command_must_index = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
case CMD_LOITER: // Just plain LOITER
|
||||
break;
|
||||
//case MAV_CMD_NAV_LOITER_UNLIM: // Just plain LOITER
|
||||
// break;
|
||||
|
||||
case CMD_ANGLE:
|
||||
|
||||
default:
|
||||
gcs.send_text(SEVERITY_HIGH,"<verify_must: default> No current Must commands");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void verify_may()
|
||||
{
|
||||
float power;
|
||||
switch(command_may_ID) {
|
||||
|
||||
case MAV_CMD_CONDITION_ANGLE:
|
||||
if((millis() - command_yaw_start_time) > command_yaw_time){
|
||||
command_must_index = 0;
|
||||
nav_yaw = command_yaw_end;
|
||||
@ -456,23 +457,14 @@ void verify_must()
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
send_message(SEVERITY_HIGH,"No current Must commands");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void verify_may()
|
||||
{
|
||||
switch(command_may_ID) {
|
||||
case CMD_DELAY:
|
||||
case MAV_CMD_CONDITION_DELAY:
|
||||
if ((millis() - delay_start) > delay_timeout){
|
||||
command_may_index = 0;
|
||||
delay_timeout = 0;
|
||||
}
|
||||
|
||||
case CMD_LAND_OPTIONS:
|
||||
break;
|
||||
//case CMD_LAND_OPTIONS:
|
||||
// break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -72,14 +72,14 @@ void read_trim_switch()
|
||||
if(trim_flag){
|
||||
// switch was just released
|
||||
if((millis() - trim_timer) > 2000){
|
||||
// not being used
|
||||
imu.save();
|
||||
|
||||
} else {
|
||||
// set the throttle nominal
|
||||
if(g.rc_3.control_in > 50){
|
||||
g.throttle_cruise = g.rc_3.control_in;
|
||||
Serial.printf("tnom %d\n", g.);
|
||||
save_EEPROM_g.();
|
||||
g.throttle_cruise.set(g.rc_3.control_in);
|
||||
Serial.printf("tnom %d\n", g.throttle_cruise.get());
|
||||
save_EEPROM_throttle_cruise();
|
||||
}
|
||||
}
|
||||
trim_flag = false;
|
||||
|
||||
@ -62,7 +62,8 @@
|
||||
#define SONAR 0
|
||||
#define BARO 1
|
||||
|
||||
#define WP_START_BYTE 0x130 // where in memory home WP is stored + all other WP
|
||||
// parameters get the first 1KiB of EEPROM, remainder is for waypoints
|
||||
#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
|
||||
#define WP_SIZE 14
|
||||
|
||||
// GCS enumeration
|
||||
@ -156,19 +157,60 @@
|
||||
#define MSG_PRESSURE 0x04
|
||||
#define MSG_STATUS_TEXT 0x05
|
||||
#define MSG_PERF_REPORT 0x06
|
||||
#define MSG_COMMAND 0x22
|
||||
#define MSG_MODE_CHANGE 0x07 //This is different than HEARTBEAT because it occurs only when the mode is actually changed
|
||||
#define MSG_VERSION_REQUEST 0x08
|
||||
#define MSG_VERSION 0x09
|
||||
#define MSG_EXTENDED_STATUS 0x0a
|
||||
#define MSG_CPU_LOAD 0x0b
|
||||
|
||||
#define MSG_COMMAND_REQUEST 0x20
|
||||
#define MSG_COMMAND_UPLOAD 0x21
|
||||
#define MSG_COMMAND_LIST 0x22
|
||||
#define MSG_COMMAND_MODE_CHANGE 0x23
|
||||
#define MSG_CURRENT_WAYPOINT 0x24
|
||||
|
||||
#define MSG_VALUE_REQUEST 0x30
|
||||
#define MSG_VALUE_SET 0x31
|
||||
#define MSG_VALUE 0x32
|
||||
|
||||
#define MSG_PID_REQUEST 0x40
|
||||
#define MSG_PID_SET 0x41
|
||||
#define MSG_PID 0x42
|
||||
#define MSG_TRIMS 0x50
|
||||
#define MSG_MINS 0x51
|
||||
#define MSG_MAXS 0x52
|
||||
#define MSG_IMU_OUT 0x53
|
||||
#define MSG_VFR_HUD 0x4a
|
||||
|
||||
#define MSG_TRIM_STARTUP 0x50
|
||||
#define MSG_TRIM_MIN 0x51
|
||||
#define MSG_TRIM_MAX 0x52
|
||||
#define MSG_RADIO_OUT 0x53
|
||||
#define MSG_RADIO_IN 0x54
|
||||
|
||||
#define MSG_RAW_IMU 0x60
|
||||
#define MSG_GPS_STATUS 0x61
|
||||
#define MSG_GPS_RAW 0x62
|
||||
|
||||
#define MSG_SERVO_OUT 0x70
|
||||
|
||||
#define MSG_PIN_REQUEST 0x80
|
||||
#define MSG_PIN_SET 0x81
|
||||
|
||||
#define MSG_DATAFLASH_REQUEST 0x90
|
||||
#define MSG_DATAFLASH_SET 0x91
|
||||
|
||||
#define MSG_EEPROM_REQUEST 0xa0
|
||||
#define MSG_EEPROM_SET 0xa1
|
||||
|
||||
#define MSG_POSITION_CORRECT 0xb0
|
||||
#define MSG_ATTITUDE_CORRECT 0xb1
|
||||
#define MSG_POSITION_SET 0xb2
|
||||
#define MSG_ATTITUDE_SET 0xb3
|
||||
#define MSG_LOCAL_LOCATION 0xb4
|
||||
|
||||
#define SEVERITY_LOW 1
|
||||
#define SEVERITY_MEDIUM 2
|
||||
#define SEVERITY_HIGH 3
|
||||
#define SEVERITY_CRITICAL 4
|
||||
|
||||
|
||||
// Logging parameters
|
||||
#define LOG_ATTITUDE_MSG 0x01
|
||||
#define LOG_GPS_MSG 0x02
|
||||
@ -269,8 +311,8 @@
|
||||
#define EE_XTRACK_GAIN 0x30
|
||||
#define EE_XTRACK_ANGLE 0x32
|
||||
#define EE_PITCH_MAX 0x34
|
||||
#define EE_DISTANCE_GAIN 0x36
|
||||
#define EE_ALTITUDE_GAIN 0x38
|
||||
//#define EE_DISTANCE_GAIN 0x36
|
||||
//#define EE_ALTITUDE_GAIN 0x38
|
||||
|
||||
#define EE_GAIN_1 0x40 // all gains stored from here
|
||||
#define EE_GAIN_2 0x48 // all gains stored from here
|
||||
|
||||
@ -1,3 +1,5 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
/*
|
||||
This event will be called when the failsafe changes
|
||||
boolean failsafe reflects the current state
|
||||
@ -18,9 +20,9 @@ void failsafe_event()
|
||||
|
||||
void low_battery_event(void)
|
||||
{
|
||||
send_message(SEVERITY_HIGH,"Low Battery!");
|
||||
gcs.send_text(SEVERITY_HIGH,"Low Battery!");
|
||||
set_mode(RTL);
|
||||
throttle_cruise = THROTTLE_CRUISE;
|
||||
g.throttle_cruise.set(THROTTLE_CRUISE);
|
||||
}
|
||||
|
||||
|
||||
@ -42,12 +44,12 @@ undo_event
|
||||
|
||||
void new_event(struct Location *cmd)
|
||||
{
|
||||
Serial.print("New Event Loaded ");
|
||||
Serial.println(cmd->p1,DEC);
|
||||
SendDebug("New Event Loaded ");
|
||||
SendDebugln(cmd->p1,DEC);
|
||||
|
||||
|
||||
if(cmd->p1 == STOP_REPEAT){
|
||||
Serial.println("STOP repeat ");
|
||||
SendDebugln("STOP repeat ");
|
||||
event_id = NO_REPEAT;
|
||||
event_timer = -1;
|
||||
undo_event = 0;
|
||||
@ -116,25 +118,26 @@ void perform_event()
|
||||
case RELAY_TOGGLE:
|
||||
|
||||
event_undo_value = PORTL & B00000100 ? 1:0;
|
||||
if(event_undo_value == 1){
|
||||
relay_A();
|
||||
}else{
|
||||
relay_B();
|
||||
}
|
||||
Serial.print("toggle relay ");
|
||||
Serial.println(PORTL,BIN);
|
||||
relay_toggle();
|
||||
SendDebug("toggle relay ");
|
||||
SendDebugln(PORTL,BIN);
|
||||
undo_event = 2;
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void relay_A()
|
||||
void relay_on()
|
||||
{
|
||||
PORTL |= B00000100;
|
||||
}
|
||||
|
||||
void relay_B()
|
||||
void relay_off()
|
||||
{
|
||||
PORTL &= ~B00000100;
|
||||
}
|
||||
|
||||
void relay_toggle()
|
||||
{
|
||||
PORTL ^= B00000100;
|
||||
}
|
||||
@ -184,14 +187,9 @@ void perform_event_undo()
|
||||
break;
|
||||
|
||||
case RELAY_TOGGLE:
|
||||
|
||||
if(event_undo_value == 1){
|
||||
relay_A();
|
||||
}else{
|
||||
relay_B();
|
||||
}
|
||||
Serial.print("untoggle relay ");
|
||||
Serial.println(PORTL,BIN);
|
||||
relay_toggle();
|
||||
SendDebug("untoggle relay ");
|
||||
SendDebugln(PORTL,BIN);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@ -274,6 +274,6 @@ set_servos_4()
|
||||
reset_I();
|
||||
|
||||
// Initialize yaw command to actual yaw when throttle is down...
|
||||
g.rc_4.control_in = ToDeg(yaw);
|
||||
g.rc_4.control_in = ToDeg(dcm.yaw);
|
||||
}
|
||||
}
|
||||
|
||||
@ -79,30 +79,28 @@ void parseCommand(char *buffer)
|
||||
case 'P':
|
||||
g.pid_stabilize_roll.kP((float)value / 1000);
|
||||
g.pid_stabilize_pitch.kP((float)value / 1000);
|
||||
save_EEPROM_PID();
|
||||
g.pid_stabilize_pitch.save_gains();
|
||||
break;
|
||||
|
||||
case 'I':
|
||||
g.pid_stabilize_roll.kI((float)value / 1000);
|
||||
g.pid_stabilize_pitch.kI((float)value / 1000);
|
||||
save_EEPROM_PID();
|
||||
g.pid_stabilize_pitch.save_gains();
|
||||
break;
|
||||
|
||||
case 'D':
|
||||
//g.pid_stabilize_roll.kD((float)value / 1000);
|
||||
//g.pid_stabilize_pitch.kD((float)value / 1000);
|
||||
//save_EEPROM_PID();
|
||||
break;
|
||||
|
||||
case 'X':
|
||||
g.pid_stabilize_roll.imax(value * 100);
|
||||
g.pid_stabilize_pitch.imax(value * 100);
|
||||
save_EEPROM_PID();
|
||||
g.pid_stabilize_pitch.save_gains();
|
||||
break;
|
||||
|
||||
case 'R':
|
||||
stabilize_dampener = (float)value / 1000;
|
||||
save_EEPROM_PID();
|
||||
g.stabilize_dampener.set_and_save((float)value / 1000);
|
||||
break;
|
||||
}
|
||||
init_pids();
|
||||
|
||||
@ -6,7 +6,7 @@ void init_pressure_ground(void)
|
||||
for(int i = 0; i < 300; i++){ // We take some readings...
|
||||
delay(20);
|
||||
APM_BMP085.Read(); // Get initial data from absolute pressure sensor
|
||||
abs_pressure_ground = (abs_pressure_ground * 9l + APM_BMP085.Press) / 10l;
|
||||
ground_pressure = (ground_pressure * 9l + APM_BMP085.Press) / 10l;
|
||||
ground_temperature = (ground_temperature * 9 + APM_BMP085.Temp) / 10;
|
||||
}
|
||||
abs_pressure = APM_BMP085.Press;
|
||||
@ -19,7 +19,7 @@ void read_barometer(void){
|
||||
|
||||
//abs_pressure = (abs_pressure + APM_BMP085.Press) >> 1; // Small filtering
|
||||
abs_pressure = ((float)abs_pressure * .7) + ((float)APM_BMP085.Press * .3); // large filtering
|
||||
scaling = (float)abs_pressure_ground / (float)abs_pressure;
|
||||
scaling = (float)ground_pressure / (float)abs_pressure;
|
||||
temp = ((float)ground_temperature / 10.f) + 273.15;
|
||||
x = log(scaling) * temp * 29271.267f;
|
||||
current_loc.alt = (long)(x / 10) + home.alt;// + baro_offset; // Pressure altitude in centimeters
|
||||
|
||||
@ -15,7 +15,7 @@ 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);
|
||||
static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
|
||||
|
||||
// Command/function table for the setup menu
|
||||
// Command/function table for the setup menu
|
||||
const struct Menu::command setup_menu_commands[] PROGMEM = {
|
||||
// command function called
|
||||
// ======= ===============
|
||||
@ -61,7 +61,7 @@ setup_show(uint8_t argc, const Menu::arg *argv)
|
||||
uint8_t i;
|
||||
// clear the area
|
||||
print_blanks(8);
|
||||
|
||||
|
||||
report_radio();
|
||||
report_frame();
|
||||
report_current();
|
||||
@ -88,14 +88,14 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
|
||||
do {
|
||||
c = Serial.read();
|
||||
} while (-1 == c);
|
||||
|
||||
|
||||
if (('y' != c) && ('Y' != c))
|
||||
return(-1);
|
||||
|
||||
|
||||
//zero_eeprom();
|
||||
default_gains();
|
||||
|
||||
|
||||
|
||||
|
||||
// setup default values
|
||||
default_waypoint_info();
|
||||
default_nav();
|
||||
@ -106,7 +106,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
|
||||
default_logs();
|
||||
default_current();
|
||||
print_done();
|
||||
|
||||
|
||||
// finish
|
||||
// ------
|
||||
return(0);
|
||||
@ -119,12 +119,12 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
Serial.println("\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){
|
||||
Serial.printf_P(PSTR("\nNo radio; Check connectors."));
|
||||
@ -132,7 +132,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
|
||||
// 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;
|
||||
@ -163,7 +163,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: "));
|
||||
while(1){
|
||||
|
||||
|
||||
delay(20);
|
||||
// Filters radio input - adjust filters in the radio.pde file
|
||||
// ----------------------------------------------------------
|
||||
@ -177,11 +177,11 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
|
||||
g.rc_6.update_min_max();
|
||||
g.rc_7.update_min_max();
|
||||
g.rc_8.update_min_max();
|
||||
|
||||
|
||||
if(Serial.available() > 0){
|
||||
//g.rc_3.radio_max += 250;
|
||||
Serial.flush();
|
||||
|
||||
|
||||
save_EEPROM_radio();
|
||||
//delay(100);
|
||||
// double checking
|
||||
@ -209,9 +209,9 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
|
||||
print_hit_enter();
|
||||
delay(1000);
|
||||
|
||||
|
||||
|
||||
int out_min = g.rc_3.radio_min + 70;
|
||||
|
||||
|
||||
|
||||
|
||||
while(1){
|
||||
@ -222,28 +222,28 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
|
||||
motor_out[CH_3] = g.rc_3.radio_min;
|
||||
motor_out[CH_4] = g.rc_3.radio_min;
|
||||
|
||||
|
||||
|
||||
if(frame_type == PLUS_FRAME){
|
||||
|
||||
|
||||
if(g.frame_type == PLUS_FRAME){
|
||||
if(g.rc_1.control_in > 0){
|
||||
motor_out[CH_1] = out_min;
|
||||
Serial.println("0");
|
||||
|
||||
|
||||
}else if(g.rc_1.control_in < 0){
|
||||
motor_out[CH_2] = out_min;
|
||||
Serial.println("1");
|
||||
}
|
||||
|
||||
|
||||
if(g.rc_2.control_in > 0){
|
||||
motor_out[CH_4] = out_min;
|
||||
Serial.println("3");
|
||||
|
||||
|
||||
}else if(g.rc_2.control_in < 0){
|
||||
motor_out[CH_3] = out_min;
|
||||
Serial.println("2");
|
||||
}
|
||||
|
||||
}else if(frame_type == X_FRAME){
|
||||
}else if(g.frame_type == X_FRAME){
|
||||
|
||||
// lower right
|
||||
if((g.rc_1.control_in > 0) && (g.rc_2.control_in > 0)){
|
||||
@ -264,36 +264,36 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
|
||||
motor_out[CH_1] = out_min;
|
||||
Serial.println("0");
|
||||
}
|
||||
|
||||
}else if(frame_type == TRI_FRAME){
|
||||
|
||||
}else if(g.frame_type == TRI_FRAME){
|
||||
|
||||
if(g.rc_1.control_in > 0){
|
||||
motor_out[CH_1] = out_min;
|
||||
|
||||
|
||||
}else if(g.rc_1.control_in < 0){
|
||||
motor_out[CH_2] = out_min;
|
||||
}
|
||||
|
||||
|
||||
if(g.rc_2.control_in > 0){
|
||||
motor_out[CH_4] = out_min;
|
||||
motor_out[CH_4] = out_min;
|
||||
}
|
||||
|
||||
if(g.rc_4.control_in > 0){
|
||||
g.rc_4.servo_out = 2000;
|
||||
|
||||
|
||||
}else if(g.rc_4.control_in < 0){
|
||||
g.rc_4.servo_out = -2000;
|
||||
}
|
||||
|
||||
|
||||
g.rc_4.calc_pwm();
|
||||
motor_out[CH_3] = g.rc_4.radio_out;
|
||||
}
|
||||
|
||||
|
||||
if(g.rc_3.control_in > 0){
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
|
||||
if(frame_type != TRI_FRAME)
|
||||
if(g.frame_type != TRI_FRAME)
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
|
||||
}else{
|
||||
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
|
||||
@ -301,7 +301,7 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
|
||||
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
|
||||
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
|
||||
}
|
||||
|
||||
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
}
|
||||
@ -312,9 +312,9 @@ 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_accel();
|
||||
imu.print_accel_offsets();
|
||||
print_accel_offsets();
|
||||
|
||||
report_imu();
|
||||
return(0);
|
||||
@ -325,14 +325,14 @@ setup_pid(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
if (!strcmp_P(argv[1].str, PSTR("default"))) {
|
||||
default_gains();
|
||||
|
||||
|
||||
}else if (!strcmp_P(argv[1].str, PSTR("s_kp"))) {
|
||||
g.pid_stabilize_roll.kP(argv[2].f);
|
||||
g.pid_stabilize_pitch.kP(argv[2].f);
|
||||
save_EEPROM_PID();
|
||||
|
||||
|
||||
}else if (!strcmp_P(argv[1].str, PSTR("s_kd"))) {
|
||||
stabilize_dampener = argv[2].f;
|
||||
g.stabilize_dampener = argv[2].f;
|
||||
save_EEPROM_PID();
|
||||
|
||||
}else if (!strcmp_P(argv[1].str, PSTR("y_kp"))) {
|
||||
@ -353,7 +353,7 @@ setup_pid(uint8_t argc, const Menu::arg *argv)
|
||||
}else{
|
||||
default_gains();
|
||||
}
|
||||
|
||||
|
||||
|
||||
report_gains();
|
||||
}
|
||||
@ -362,20 +362,20 @@ static int8_t
|
||||
setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
byte switchPosition, oldSwitchPosition, mode;
|
||||
|
||||
|
||||
Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
|
||||
print_hit_enter();
|
||||
trim_radio();
|
||||
|
||||
|
||||
while(1){
|
||||
delay(20);
|
||||
read_radio();
|
||||
read_radio();
|
||||
switchPosition = readSwitch();
|
||||
|
||||
|
||||
|
||||
// look for control switch change
|
||||
if (oldSwitchPosition != switchPosition){
|
||||
|
||||
|
||||
mode = g.flight_modes[switchPosition];
|
||||
mode = constrain(mode, 0, NUM_MODES-1);
|
||||
|
||||
@ -383,12 +383,12 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
print_switch(switchPosition, mode);
|
||||
|
||||
// Remember switch position
|
||||
oldSwitchPosition = switchPosition;
|
||||
oldSwitchPosition = switchPosition;
|
||||
}
|
||||
|
||||
// look for stick input
|
||||
if (radio_input_switch() == true){
|
||||
mode++;
|
||||
mode++;
|
||||
if(mode >= NUM_MODES)
|
||||
mode = 0;
|
||||
|
||||
@ -398,7 +398,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
// print new mode
|
||||
print_switch(switchPosition, mode);
|
||||
}
|
||||
|
||||
|
||||
// escape hatch
|
||||
if(Serial.available() > 0){
|
||||
save_EEPROM_flight_modes();
|
||||
@ -412,8 +412,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
static int8_t
|
||||
setup_declination(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
mag_declination = argv[1].f;
|
||||
save_EEPROM_mag_declination();
|
||||
compass.set_declination(radians(argv[1].f));
|
||||
report_compass();
|
||||
}
|
||||
|
||||
@ -430,16 +429,16 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
|
||||
if (!strcmp_P(argv[1].str, PSTR("on"))) {
|
||||
g.compass_enabled = true;
|
||||
init_compass();
|
||||
|
||||
|
||||
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
||||
g.compass_enabled = false;
|
||||
|
||||
|
||||
} else {
|
||||
Serial.printf_P(PSTR("\nOptions:[on,off]\n"));
|
||||
report_compass();
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
save_EEPROM_mag();
|
||||
report_compass();
|
||||
return 0;
|
||||
@ -449,23 +448,23 @@ static int8_t
|
||||
setup_frame(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
if (!strcmp_P(argv[1].str, PSTR("+"))) {
|
||||
frame_type = PLUS_FRAME;
|
||||
g.frame_type = PLUS_FRAME;
|
||||
|
||||
} else if (!strcmp_P(argv[1].str, PSTR("x"))) {
|
||||
frame_type = X_FRAME;
|
||||
|
||||
g.frame_type = X_FRAME;
|
||||
|
||||
} else if (!strcmp_P(argv[1].str, PSTR("tri"))) {
|
||||
frame_type = TRI_FRAME;
|
||||
g.frame_type = TRI_FRAME;
|
||||
|
||||
} else if (!strcmp_P(argv[1].str, PSTR("hexa"))) {
|
||||
frame_type = HEXA_FRAME;
|
||||
g.frame_type = HEXA_FRAME;
|
||||
|
||||
} else {
|
||||
Serial.printf_P(PSTR("\nOptions:[+, x, tri, hexa]\n"));
|
||||
report_frame();
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
save_EEPROM_frame();
|
||||
report_frame();
|
||||
return 0;
|
||||
@ -475,22 +474,22 @@ static int8_t
|
||||
setup_current(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
if (!strcmp_P(argv[1].str, PSTR("on"))) {
|
||||
current_enabled = true;
|
||||
g.current_enabled.set(true);
|
||||
save_EEPROM_mag();
|
||||
|
||||
|
||||
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
||||
current_enabled = false;
|
||||
g.current_enabled.set(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;
|
||||
@ -509,7 +508,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
|
||||
compass.set_orientation(MAGORIENTATION); // set compass's orientation on aircraft
|
||||
compass.set_offsets(0, 0, 0); // set offsets to account for surrounding interference
|
||||
compass.set_declination(ToRad(DECLINATION)); // set local difference between magnetic north and true north
|
||||
//int counter = 0;
|
||||
//int counter = 0;
|
||||
float _min[3], _max[3], _offset[3];
|
||||
|
||||
while(1){
|
||||
@ -537,15 +536,15 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
|
||||
offset[0] = -(_max[0] + _min[0]) / 2;
|
||||
offset[1] = -(_max[1] + _min[1]) / 2;
|
||||
offset[2] = -(_max[2] + _min[2]) / 2;
|
||||
|
||||
// display all to user
|
||||
|
||||
// display all to user
|
||||
Serial.printf_P(PSTR("Heading: "));
|
||||
Serial.print(ToDeg(compass.heading));
|
||||
Serial.print(" \t(");
|
||||
Serial.print(compass.mag_x);
|
||||
Serial.print(",");
|
||||
Serial.print(compass.mag_y);
|
||||
Serial.print(",");
|
||||
Serial.print(",");
|
||||
Serial.print(compass.mag_z);
|
||||
Serial.print(")\t offsets(");
|
||||
Serial.print(offset[0]);
|
||||
@ -554,18 +553,18 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
|
||||
Serial.print(",");
|
||||
Serial.print(offset[2]);
|
||||
Serial.println(")");
|
||||
|
||||
|
||||
if(Serial.available() > 0){
|
||||
|
||||
|
||||
//mag_offset_x = offset[0];
|
||||
//mag_offset_y = offset[1];
|
||||
//mag_offset_z = offset[2];
|
||||
|
||||
|
||||
//save_EEPROM_mag_offset();
|
||||
|
||||
|
||||
// set offsets to account for surrounding interference
|
||||
//compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);
|
||||
|
||||
|
||||
report_compass();
|
||||
break;
|
||||
}
|
||||
@ -579,7 +578,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
|
||||
/***************************************************************************/
|
||||
|
||||
void default_waypoint_info()
|
||||
{
|
||||
{
|
||||
g.waypoint_radius = 4; //TODO: Replace this quick fix with a real way to define wp_radius
|
||||
g.loiter_radius = 30; //TODO: Replace this quick fix with a real way to define loiter_radius
|
||||
save_EEPROM_waypoint_info();
|
||||
@ -599,14 +598,14 @@ default_nav()
|
||||
void
|
||||
default_alt_hold()
|
||||
{
|
||||
alt_to_hold = -1;
|
||||
g.RTL_altitude.set(-1);
|
||||
save_EEPROM_alt_RTL();
|
||||
}
|
||||
|
||||
void
|
||||
default_frame()
|
||||
{
|
||||
frame_type = PLUS_FRAME;
|
||||
g.frame_type = PLUS_FRAME;
|
||||
save_EEPROM_frame();
|
||||
}
|
||||
|
||||
@ -614,7 +613,7 @@ void
|
||||
default_current()
|
||||
{
|
||||
milliamp_hours = 2000;
|
||||
current_enabled = false;
|
||||
g.current_enabled.set(false);
|
||||
save_EEPROM_current();
|
||||
}
|
||||
|
||||
@ -633,12 +632,12 @@ default_flight_modes()
|
||||
void
|
||||
default_throttle()
|
||||
{
|
||||
g.throttle_min = THROTTLE_MIN;
|
||||
g.throttle_max = THROTTLE_MAX;
|
||||
g.throttle_cruise = THROTTLE_CRUISE;
|
||||
throttle_failsafe_enabled = THROTTLE_FAILSAFE;
|
||||
throttle_failsafe_action = THROTTLE_FAILSAFE_ACTION;
|
||||
throttle_failsafe_value = THROTTLE_FS_VALUE;
|
||||
g.throttle_min = THROTTLE_MIN;
|
||||
g.throttle_max = THROTTLE_MAX;
|
||||
g.throttle_cruise = THROTTLE_CRUISE;
|
||||
g.throttle_failsafe_enabled = THROTTLE_FAILSAFE;
|
||||
g.throttle_failsafe_action = THROTTLE_FAILSAFE_ACTION;
|
||||
g.throttle_failsafe_value = THROTTLE_FS_VALUE;
|
||||
save_EEPROM_throttle();
|
||||
}
|
||||
|
||||
@ -647,7 +646,7 @@ void default_logs()
|
||||
|
||||
// convenience macro for testing LOG_* and setting LOGBIT_*
|
||||
#define LOGBIT(_s) (LOG_ ## _s ? LOGBIT_ ## _s : 0)
|
||||
g.log_bitmask =
|
||||
g.log_bitmask =
|
||||
LOGBIT(ATTITUDE_FAST) |
|
||||
LOGBIT(ATTITUDE_MED) |
|
||||
LOGBIT(GPS) |
|
||||
@ -659,7 +658,7 @@ void default_logs()
|
||||
LOGBIT(CMD) |
|
||||
LOGBIT(CURRENT);
|
||||
#undef LOGBIT
|
||||
|
||||
|
||||
save_EEPROM_logs();
|
||||
}
|
||||
|
||||
@ -672,7 +671,7 @@ default_gains()
|
||||
g.pid_acro_rate_roll.kI(ACRO_RATE_ROLL_I);
|
||||
g.pid_acro_rate_roll.kD(0);
|
||||
g.pid_acro_rate_roll.imax(ACRO_RATE_ROLL_IMAX * 100);
|
||||
|
||||
|
||||
g.pid_acro_rate_pitch.kP(ACRO_RATE_PITCH_P);
|
||||
g.pid_acro_rate_pitch.kI(ACRO_RATE_PITCH_I);
|
||||
g.pid_acro_rate_pitch.kD(0);
|
||||
@ -689,7 +688,7 @@ default_gains()
|
||||
g.pid_stabilize_roll.kI(STABILIZE_ROLL_I);
|
||||
g.pid_stabilize_roll.kD(0);
|
||||
g.pid_stabilize_roll.imax(STABILIZE_ROLL_IMAX * 100);
|
||||
|
||||
|
||||
g.pid_stabilize_pitch.kP(STABILIZE_PITCH_P);
|
||||
g.pid_stabilize_pitch.kI(STABILIZE_PITCH_I);
|
||||
g.pid_stabilize_pitch.kD(0);
|
||||
@ -704,10 +703,10 @@ default_gains()
|
||||
|
||||
// custom dampeners
|
||||
// roll pitch
|
||||
stabilize_dampener = STABILIZE_DAMPENER;
|
||||
|
||||
g.stabilize_dampener = STABILIZE_DAMPENER;
|
||||
|
||||
//yaw
|
||||
hold_yaw_dampener = HOLD_YAW_DAMPENER;
|
||||
g.hold_yaw_dampener = HOLD_YAW_DAMPENER;
|
||||
|
||||
|
||||
// navigation
|
||||
@ -730,7 +729,7 @@ default_gains()
|
||||
g.pid_sonar_throttle.kI(THROTTLE_SONAR_I);
|
||||
g.pid_sonar_throttle.kD(THROTTLE_SONAR_D);
|
||||
g.pid_sonar_throttle.imax(THROTTLE_SONAR_IMAX);
|
||||
|
||||
|
||||
save_EEPROM_PID();
|
||||
}
|
||||
|
||||
@ -746,7 +745,7 @@ void report_current()
|
||||
read_EEPROM_current();
|
||||
Serial.printf_P(PSTR("Current Sensor\n"));
|
||||
print_divider();
|
||||
print_enabled(current_enabled);
|
||||
print_enabled(g.current_enabled.get());
|
||||
|
||||
Serial.printf_P(PSTR("mah: %d"),milliamp_hours);
|
||||
print_blanks(1);
|
||||
@ -760,17 +759,17 @@ void report_frame()
|
||||
Serial.printf_P(PSTR("Frame\n"));
|
||||
print_divider();
|
||||
|
||||
|
||||
if(frame_type == X_FRAME)
|
||||
|
||||
if(g.frame_type == X_FRAME)
|
||||
Serial.printf_P(PSTR("X "));
|
||||
else if(frame_type == PLUS_FRAME)
|
||||
else if(g.frame_type == PLUS_FRAME)
|
||||
Serial.printf_P(PSTR("Plus "));
|
||||
else if(frame_type == TRI_FRAME)
|
||||
else if(g.frame_type == TRI_FRAME)
|
||||
Serial.printf_P(PSTR("TRI "));
|
||||
else if(frame_type == HEXA_FRAME)
|
||||
else if(g.frame_type == HEXA_FRAME)
|
||||
Serial.printf_P(PSTR("HEXA "));
|
||||
|
||||
Serial.printf_P(PSTR("frame (%d)"), (int)frame_type);
|
||||
Serial.printf_P(PSTR("frame (%d)"), (int)g.frame_type);
|
||||
print_blanks(1);
|
||||
}
|
||||
|
||||
@ -807,10 +806,10 @@ void report_gains()
|
||||
print_PID(&g.pid_stabilize_pitch);
|
||||
Serial.printf_P(PSTR("yaw:\n"));
|
||||
print_PID(&g.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);
|
||||
|
||||
|
||||
Serial.printf_P(PSTR("Stabilize dampener: %4.3f\n"), g.stabilize_dampener);
|
||||
Serial.printf_P(PSTR("Yaw Dampener: %4.3f\n\n"), g.hold_yaw_dampener);
|
||||
|
||||
// Nav
|
||||
Serial.printf_P(PSTR("Nav:\nlat:\n"));
|
||||
print_PID(&g.pid_nav_lat);
|
||||
@ -833,9 +832,9 @@ void report_xtrack()
|
||||
Serial.printf_P(PSTR("XTRACK: %4.2f\n"
|
||||
"XTRACK angle: %d\n"
|
||||
"PITCH_MAX: %ld"),
|
||||
(float)g.crosstrack_gain,
|
||||
(int)g.crosstrack_entry_angle,
|
||||
(long)g.pitch_max);
|
||||
g.crosstrack_gain,
|
||||
g.crosstrack_entry_angle,
|
||||
g.pitch_max);
|
||||
print_blanks(1);
|
||||
}
|
||||
|
||||
@ -851,11 +850,11 @@ void report_throttle()
|
||||
"cruise: %d\n"
|
||||
"failsafe_enabled: %d\n"
|
||||
"failsafe_value: %d"),
|
||||
(int)g.throttle_min,
|
||||
(int)g.throttle_max,
|
||||
(int)g.throttle_cruise,
|
||||
throttle_failsafe_enabled,
|
||||
throttle_failsafe_value);
|
||||
g.throttle_min,
|
||||
g.throttle_max,
|
||||
g.throttle_cruise,
|
||||
g.throttle_failsafe_enabled,
|
||||
g.throttle_failsafe_value);
|
||||
print_blanks(1);
|
||||
}
|
||||
|
||||
@ -865,43 +864,45 @@ void report_imu()
|
||||
Serial.printf_P(PSTR("IMU\n"));
|
||||
print_divider();
|
||||
|
||||
imu.print_gyro_offsets();
|
||||
imu.print_accel_offsets();
|
||||
print_gyro_offsets();
|
||||
print_accel_offsets();
|
||||
print_blanks(1);
|
||||
}
|
||||
|
||||
void report_compass()
|
||||
{
|
||||
print_blanks(2);
|
||||
print_blanks(2);
|
||||
Serial.printf_P(PSTR("Compass\n"));
|
||||
print_divider();
|
||||
|
||||
read_EEPROM_compass();
|
||||
read_EEPROM_compass_declination();
|
||||
read_EEPROM_compass_offset();
|
||||
//read_EEPROM_compass_declination();
|
||||
//read_EEPROM_compass_offset();
|
||||
|
||||
print_enabled(g.compass_enabled);
|
||||
|
||||
|
||||
// mag declination
|
||||
Serial.printf_P(PSTR("Mag Delination: %4.4f\n"),
|
||||
mag_declination);
|
||||
Serial.printf_P(PSTR("Mag Delination: %4.4f\n"),
|
||||
degrees(compass.get_declination()));
|
||||
|
||||
Vector3f offsets = compass.get_offsets();
|
||||
|
||||
// mag offsets
|
||||
Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f"),
|
||||
mag_offset_x,
|
||||
mag_offset_y,
|
||||
mag_offset_z);
|
||||
offsets.x,
|
||||
offsets.y,
|
||||
offsets.z);
|
||||
print_blanks(1);
|
||||
}
|
||||
|
||||
|
||||
void report_flight_modes()
|
||||
{
|
||||
print_blanks(2);
|
||||
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, g.flight_modes[i]);
|
||||
}
|
||||
@ -912,23 +913,23 @@ void report_flight_modes()
|
||||
// CLI utilities
|
||||
/***************************************************************************/
|
||||
|
||||
void
|
||||
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());
|
||||
}
|
||||
|
||||
void
|
||||
void
|
||||
print_radio_values()
|
||||
{
|
||||
Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max);
|
||||
Serial.printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max);
|
||||
Serial.printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max);
|
||||
Serial.printf_P(PSTR("CH4: %d | %d\n"), (int)g.rc_4.radio_min, (int)g.rc_4.radio_max);
|
||||
Serial.printf_P(PSTR("CH5: %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_max);
|
||||
Serial.printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max);
|
||||
Serial.printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max);
|
||||
Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
|
||||
Serial.printf_P(PSTR("CH1: %d | %d\n"), g.rc_1.radio_min, g.rc_1.radio_max);
|
||||
Serial.printf_P(PSTR("CH2: %d | %d\n"), g.rc_2.radio_min, g.rc_2.radio_max);
|
||||
Serial.printf_P(PSTR("CH3: %d | %d\n"), g.rc_3.radio_min, g.rc_3.radio_max);
|
||||
Serial.printf_P(PSTR("CH4: %d | %d\n"), g.rc_4.radio_min, g.rc_4.radio_max);
|
||||
Serial.printf_P(PSTR("CH5: %d | %d\n"), g.rc_5.radio_min, g.rc_5.radio_max);
|
||||
Serial.printf_P(PSTR("CH6: %d | %d\n"), g.rc_6.radio_min, g.rc_6.radio_max);
|
||||
Serial.printf_P(PSTR("CH7: %d | %d\n"), g.rc_7.radio_min, g.rc_7.radio_max);
|
||||
Serial.printf_P(PSTR("CH8: %d | %d\n"), g.rc_8.radio_min, g.rc_8.radio_max);
|
||||
}
|
||||
|
||||
void
|
||||
@ -968,7 +969,7 @@ boolean
|
||||
radio_input_switch(void)
|
||||
{
|
||||
static byte bouncer;
|
||||
|
||||
|
||||
if (abs(g.rc_1.radio_in - g.rc_1.radio_trim) > 200)
|
||||
bouncer = 10;
|
||||
|
||||
@ -1005,3 +1006,26 @@ void print_enabled(boolean b)
|
||||
|
||||
|
||||
|
||||
void
|
||||
print_accel_offsets(void)
|
||||
{
|
||||
//Serial.print("Accel offsets: ");
|
||||
//Serial.print(imu._adc_offset[3], 2);
|
||||
//Serial.print(", ");
|
||||
//Serial.print(_adc_offset[4], 2);
|
||||
//Serial.print(", ");
|
||||
//Serial.println(_adc_offset[5], 2);
|
||||
}
|
||||
|
||||
void
|
||||
print_gyro_offsets(void)
|
||||
{
|
||||
//Serial.print("Gyro offsets: ");
|
||||
//Serial.print(_adc_offset[0], 2);
|
||||
//Serial.print(", ");
|
||||
//Serial.print(_adc_offset[1], 2);
|
||||
//Serial.print(", ");
|
||||
//Serial.println(_adc_offset[2], 2);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -85,7 +85,7 @@ void init_ardupilot()
|
||||
#endif
|
||||
|
||||
Serial.printf_P(PSTR("\n\n"
|
||||
"Init ArduCopterMega 1.0.2 Public Alpha\n\n"
|
||||
"Init ArduCopterMega 1.0.3 Public Alpha\n\n"
|
||||
#if TELEMETRY_PORT == 3
|
||||
"Telemetry is on the xbee port\n"
|
||||
#endif
|
||||
@ -125,6 +125,9 @@ void init_ardupilot()
|
||||
|
||||
g_gps = &GPS;
|
||||
g_gps->init();
|
||||
|
||||
imu.init(IMU::WARM_START); // offsets are loaded from EEPROM
|
||||
|
||||
|
||||
if(g.compass_enabled)
|
||||
init_compass();
|
||||
@ -243,7 +246,7 @@ void startup_ground(void)
|
||||
|
||||
// Warm up and read Gyro offsets
|
||||
// -----------------------------
|
||||
imu.init_gyro();
|
||||
imu.init(IMU::COLD_START);
|
||||
|
||||
// Save the settings for in-air restart
|
||||
// ------------------------------------
|
||||
@ -403,10 +406,9 @@ void
|
||||
init_compass()
|
||||
{
|
||||
dcm.set_compass(&compass);
|
||||
compass.init();
|
||||
bool junkbool = compass.init();
|
||||
compass.set_orientation(MAGORIENTATION); // set compass's orientation on aircraft
|
||||
compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z); // set offsets to account for surrounding interference
|
||||
compass.set_declination(ToRad(mag_declination)); // set local difference between magnetic north and true north
|
||||
Vector3f junkvector = compass.get_offsets(); // load offsets to account for airframe magnetic interference
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -686,15 +686,15 @@ test_relay(uint8_t argc, const Menu::arg *argv)
|
||||
delay(1000);
|
||||
|
||||
while(1){
|
||||
Serial.println("Relay A");
|
||||
relay_A();
|
||||
Serial.println("Relay on");
|
||||
relay_on();
|
||||
delay(3000);
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
}
|
||||
|
||||
Serial.println("Relay B");
|
||||
relay_B();
|
||||
Serial.println("Relay off");
|
||||
relay_off();
|
||||
delay(3000);
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
@ -764,7 +764,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
|
||||
sum += abs_pressure;
|
||||
delay(10);
|
||||
}
|
||||
abs_pressure_ground = (float)sum / 100.0;
|
||||
ground_pressure = (float)sum / 100.0;
|
||||
*/
|
||||
|
||||
home.alt = 0;
|
||||
@ -795,7 +795,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
|
||||
current_loc.alt,
|
||||
next_WP.alt,
|
||||
altitude_error,
|
||||
g.,
|
||||
g.throttle_cruise,
|
||||
g.rc_3.servo_out);
|
||||
|
||||
/*
|
||||
|
||||
Loading…
Reference in New Issue
Block a user