Should be flyable now. Please Use Reset in setup before flying.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1726 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
5d81f37b37
commit
957e1a747b
@ -6,11 +6,9 @@
|
||||
|
||||
#define GCS_PROTOCOL GCS_PROTOCOL_NONE
|
||||
|
||||
//#define MAGORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK
|
||||
#define MAGORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
|
||||
//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK
|
||||
|
||||
# define SERIAL0_BAUD 38400
|
||||
//# define SERIAL0_BAUD 115200
|
||||
|
||||
//# define STABILIZE_ROLL_P 0.4
|
||||
//# define STABILIZE_PITCH_P 0.4
|
||||
@ -24,15 +22,6 @@
|
||||
|
||||
|
||||
|
||||
// For future development, don't enable unless you know them
|
||||
// These are all experimental and underwork, jp 23-12-10
|
||||
//#define ENABLE_EXTRAS ENABLED
|
||||
//#define ENABLE_EXTRAINIT ENABLED
|
||||
//#define ENABLE_CAM ENABLED
|
||||
//#define ENABLE_AM ENABLED
|
||||
//#define ENABLE_xx ENABLED
|
||||
|
||||
|
||||
// Logging
|
||||
//#define LOG_CURRENT ENABLED
|
||||
|
||||
|
||||
@ -91,43 +91,52 @@ GPS *g_gps;
|
||||
|
||||
#if HIL_MODE == HIL_MODE_NONE
|
||||
|
||||
// real sensors
|
||||
AP_ADC_ADS7844 adc;
|
||||
APM_BMP085_Class barometer;
|
||||
AP_Compass_HMC5843 compass(Parameters::k_param_compass);
|
||||
// real sensors
|
||||
AP_ADC_ADS7844 adc;
|
||||
APM_BMP085_Class barometer;
|
||||
AP_Compass_HMC5843 compass(Parameters::k_param_compass);
|
||||
|
||||
// real GPS selection
|
||||
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
|
||||
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
|
||||
AP_GPS_NMEA g_gps_driver(&Serial1);
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
|
||||
AP_GPS_SIRF g_gps_driver(&Serial1);
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
|
||||
AP_GPS_UBLOX g_gps_driver(&Serial1);
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
|
||||
AP_GPS_MTK g_gps_driver(&Serial1);
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
|
||||
AP_GPS_MTK16 g_gps_driver(&Serial1);
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
|
||||
AP_GPS_None g_gps_driver(NULL);
|
||||
#else
|
||||
#error Unrecognised GPS_PROTOCOL setting.
|
||||
#endif // GPS PROTOCOL
|
||||
// real GPS selection
|
||||
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
|
||||
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
|
||||
AP_GPS_NMEA g_gps_driver(&Serial1);
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
|
||||
AP_GPS_SIRF g_gps_driver(&Serial1);
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
|
||||
AP_GPS_UBLOX g_gps_driver(&Serial1);
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
|
||||
AP_GPS_MTK g_gps_driver(&Serial1);
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
|
||||
AP_GPS_MTK16 g_gps_driver(&Serial1);
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
|
||||
AP_GPS_None g_gps_driver(NULL);
|
||||
|
||||
#else
|
||||
#error Unrecognised GPS_PROTOCOL setting.
|
||||
#endif // GPS PROTOCOL
|
||||
|
||||
#elif HIL_MODE == HIL_MODE_SENSORS
|
||||
// sensor emulators
|
||||
AP_ADC_HIL adc;
|
||||
APM_BMP085_HIL_Class barometer;
|
||||
AP_Compass_HIL compass;
|
||||
AP_GPS_HIL g_gps_driver(NULL);
|
||||
// sensor emulators
|
||||
AP_ADC_HIL adc;
|
||||
APM_BMP085_HIL_Class barometer;
|
||||
AP_Compass_HIL compass;
|
||||
AP_GPS_HIL g_gps_driver(NULL);
|
||||
|
||||
#elif HIL_MODE == HIL_MODE_ATTITUDE
|
||||
AP_DCM_HIL dcm;
|
||||
AP_GPS_HIL g_gps_driver(NULL);
|
||||
AP_DCM_HIL dcm;
|
||||
AP_GPS_HIL g_gps_driver(NULL);
|
||||
AP_Compass_HIL compass; // never used
|
||||
AP_IMU_Shim imu; // never used
|
||||
|
||||
#else
|
||||
#error Unrecognised HIL_MODE setting.
|
||||
#error Unrecognised HIL_MODE setting.
|
||||
#endif // HIL MODE
|
||||
|
||||
#if HIL_MODE != HIL_MODE_DISABLED
|
||||
@ -152,10 +161,10 @@ AP_GPS_HIL g_gps_driver(NULL);
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
|
||||
GCS_MAVLINK gcs;
|
||||
GCS_MAVLINK gcs;
|
||||
#else
|
||||
// If we are not using a GCS, we need a stub that does nothing.
|
||||
GCS_Class gcs;
|
||||
// If we are not using a GCS, we need a stub that does nothing.
|
||||
GCS_Class gcs;
|
||||
#endif
|
||||
|
||||
AP_RangeFinder_MaxsonarXL sonar;
|
||||
@ -165,8 +174,6 @@ AP_RangeFinder_MaxsonarXL sonar;
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
byte control_mode = STABILIZE;
|
||||
boolean failsafe = false; // did our throttle dip below the failsafe value?
|
||||
boolean ch3_failsafe = false;
|
||||
byte oldSwitchPosition; // for remembering the control mode switch
|
||||
|
||||
const char *comma = ",";
|
||||
@ -177,6 +184,7 @@ const char* flight_mode_strings[] = {
|
||||
"ALT_HOLD",
|
||||
"FBW",
|
||||
"AUTO",
|
||||
"LOITER",
|
||||
"POSITION_HOLD",
|
||||
"RTL",
|
||||
"TAKEOFF",
|
||||
@ -189,7 +197,7 @@ const char* flight_mode_strings[] = {
|
||||
3 Throttle
|
||||
4 Rudder (if we have ailerons)
|
||||
5 Mode - 3 position switch
|
||||
6 Altitude for Hold, user assignable
|
||||
6 User assignable
|
||||
7 trainer switch - sets throttle nominal (toggle switch), sets accels to Level (hold > 1 second)
|
||||
8 TBD
|
||||
*/
|
||||
@ -199,7 +207,15 @@ const char* flight_mode_strings[] = {
|
||||
int motor_out[4];
|
||||
Vector3f omega;
|
||||
|
||||
// Failsafe
|
||||
// --------
|
||||
boolean failsafe; // did our throttle dip below the failsafe value?
|
||||
boolean ch3_failsafe;
|
||||
boolean motor_armed;
|
||||
boolean motor_auto_safe;
|
||||
|
||||
// PIDs
|
||||
// ----
|
||||
int max_stabilize_dampener; //
|
||||
int max_yaw_dampener; //
|
||||
boolean rate_yaw_flag; // used to transition yaw control from Rate control to Yaw hold
|
||||
@ -209,8 +225,6 @@ boolean rate_yaw_flag; // used to transition yaw control from Rate control
|
||||
boolean motor_light; // status of the Motor safety
|
||||
boolean GPS_light; // status of the GPS light
|
||||
|
||||
float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
|
||||
|
||||
// GPS variables
|
||||
// -------------
|
||||
const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage
|
||||
@ -218,51 +232,41 @@ float scaleLongUp = 1; // used to reverse longtitude scaling
|
||||
float scaleLongDown = 1; // used to reverse longtitude scaling
|
||||
byte ground_start_count = 5; // have we achieved first lock and set Home?
|
||||
|
||||
// Magnetometer variables
|
||||
// ----------------------
|
||||
Vector3f mag_offsets;
|
||||
|
||||
// Location & Navigation
|
||||
// ---------------------
|
||||
const float radius_of_earth = 6378100; // meters
|
||||
const float gravity = 9.81; // meters/ sec^2
|
||||
//byte wp_radius = 3; // meters
|
||||
const float radius_of_earth = 6378100; // meters
|
||||
const float gravity = 9.81; // meters/ sec^2
|
||||
long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
|
||||
long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
|
||||
long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
|
||||
int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
|
||||
long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
|
||||
long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
|
||||
int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
|
||||
float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
|
||||
|
||||
byte command_must_index; // current command memory location
|
||||
byte command_may_index; // current command memory location
|
||||
byte command_must_ID; // current command ID
|
||||
byte command_may_ID; // current command ID
|
||||
|
||||
float cos_roll_x, sin_roll_y;
|
||||
float cos_pitch_x, sin_pitch_y;
|
||||
float sin_yaw_y, cos_yaw_x;
|
||||
float cos_roll_x = 1;
|
||||
float cos_pitch_x = 1;
|
||||
float cos_yaw_x = 1;
|
||||
float sin_pitch_y, sin_yaw_y, sin_roll_y;
|
||||
|
||||
// Airspeed
|
||||
// --------
|
||||
int airspeed; // m/s * 100
|
||||
float airspeed_error; // m / s * 100
|
||||
|
||||
// Throttle Failsafe
|
||||
// ------------------
|
||||
boolean motor_armed = false;
|
||||
boolean motor_auto_safe = false;
|
||||
int airspeed; // m/s * 100
|
||||
|
||||
// Location Errors
|
||||
// ---------------
|
||||
long bearing_error; // deg * 100 : 0 to 36000
|
||||
long altitude_error; // meters * 100 we are off in altitude
|
||||
long bearing_error; // deg * 100 : 0 to 36000
|
||||
long altitude_error; // meters * 100 we are off in altitude
|
||||
float crosstrack_error; // meters we are off trackline
|
||||
long distance_error; // distance to the WP
|
||||
long yaw_error; // how off are we pointed
|
||||
long long_error, lat_error; // temp for debugging
|
||||
|
||||
long long_error, lat_error; // temp fro debugging
|
||||
|
||||
// Sensors
|
||||
// -------
|
||||
// Battery Sensors
|
||||
// ---------------
|
||||
float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter
|
||||
float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter
|
||||
float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter
|
||||
@ -272,19 +276,9 @@ 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;
|
||||
//int milliamp_hours;
|
||||
//boolean current_enabled = false;
|
||||
|
||||
// Magnetometer variables
|
||||
// ----------------------
|
||||
//int magnetom_x;
|
||||
//int magnetom_y;
|
||||
//int magnetom_z;
|
||||
|
||||
//float mag_offset_x;
|
||||
//float mag_offset_y;
|
||||
//float mag_offset_z;
|
||||
//float mag_declination;
|
||||
// Airspeed Sensors
|
||||
// ----------------
|
||||
|
||||
// Barometer Sensor variables
|
||||
// --------------------------
|
||||
@ -292,20 +286,18 @@ unsigned long abs_pressure;
|
||||
unsigned long ground_pressure;
|
||||
int ground_temperature;
|
||||
|
||||
byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
|
||||
|
||||
// Sonar Sensor variables
|
||||
// Altitude Sensor variables
|
||||
// ----------------------
|
||||
long sonar_alt;
|
||||
long baro_alt;
|
||||
|
||||
byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
|
||||
|
||||
// flight mode specific
|
||||
// --------------------
|
||||
boolean takeoff_complete = false; // Flag for using take-off controls
|
||||
boolean land_complete = false;
|
||||
int takeoff_altitude;
|
||||
int landing_distance; // meters;
|
||||
boolean takeoff_complete; // Flag for using take-off controls
|
||||
boolean land_complete;
|
||||
int takeoff_altitude;
|
||||
int landing_distance; // meters;
|
||||
long old_alt; // used for managing altitude rates
|
||||
int velocity_land;
|
||||
|
||||
@ -320,13 +312,13 @@ int loiter_time_max; // millis : how long to stay in LOITER mode
|
||||
|
||||
// these are the values for navigation control functions
|
||||
// ----------------------------------------------------
|
||||
long nav_roll; // deg * 100 : target roll angle
|
||||
long nav_pitch; // deg * 100 : target pitch angle
|
||||
long nav_yaw; // deg * 100 : target yaw angle
|
||||
long nav_lat; // for error calcs
|
||||
long nav_lon; // for error calcs
|
||||
int nav_throttle; // 0-1000 for throttle control
|
||||
int nav_throttle_old; // for filtering
|
||||
long nav_roll; // deg * 100 : target roll angle
|
||||
long nav_pitch; // deg * 100 : target pitch angle
|
||||
long nav_yaw; // deg * 100 : target yaw angle
|
||||
long nav_lat; // for error calcs
|
||||
long nav_lon; // for error calcs
|
||||
int nav_throttle; // 0-1000 for throttle control
|
||||
int nav_throttle_old; // for filtering
|
||||
|
||||
long command_yaw_start; // what angle were we to begin with
|
||||
long command_yaw_start_time; // when did we start turning
|
||||
@ -338,12 +330,9 @@ byte command_yaw_dir;
|
||||
|
||||
// Waypoints
|
||||
// ---------
|
||||
//long GPS_wp_distance; // meters - distance between plane and next waypoint
|
||||
long wp_distance; // meters - distance between plane and next waypoint
|
||||
long wp_totalDistance; // meters - distance between old and next waypoint
|
||||
//byte wp_total; // # of Commands total including way
|
||||
//byte wp_index; // Current active command index
|
||||
byte next_wp_index; // Current active command index
|
||||
long wp_distance; // meters - distance between plane and next waypoint
|
||||
long wp_totalDistance; // meters - distance between old and next waypoint
|
||||
byte next_wp_index; // Current active command index
|
||||
|
||||
// repeating event control
|
||||
// -----------------------
|
||||
@ -376,14 +365,14 @@ boolean home_is_set; // Flag for if we have g_gps lock and have set the ho
|
||||
|
||||
// IMU variables
|
||||
// -------------
|
||||
float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm)
|
||||
float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm)
|
||||
|
||||
|
||||
// Performance monitoring
|
||||
// ----------------------
|
||||
long perf_mon_timer;
|
||||
float imu_health; // Metric based on accel gain deweighting
|
||||
int G_Dt_max; // Max main loop cycle time in milliseconds
|
||||
float imu_health; // Metric based on accel gain deweighting
|
||||
int G_Dt_max; // Max main loop cycle time in milliseconds
|
||||
byte gyro_sat_count;
|
||||
byte adc_constraints;
|
||||
byte renorm_sqrt_count;
|
||||
@ -395,7 +384,7 @@ byte gcs_messages_sent;
|
||||
// GCS
|
||||
// ---
|
||||
char GCS_buffer[53];
|
||||
char display_PID = -1; // Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed
|
||||
char display_PID = -1; // Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed
|
||||
|
||||
// System Timers
|
||||
// --------------
|
||||
@ -415,12 +404,12 @@ byte fbw_timer; // for limiting the execution of FBW input
|
||||
unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav
|
||||
unsigned long nav2_loopTimer; // used to track the elapsed ime for GPS nav
|
||||
|
||||
|
||||
unsigned long dTnav; // Delta Time in milliseconds for navigation computations
|
||||
unsigned long dTnav2; // Delta Time in milliseconds for navigation computations
|
||||
unsigned long elapsedTime; // for doing custom events
|
||||
float load; // % MCU cycles used
|
||||
|
||||
byte counter_one_herz;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Top-level logic
|
||||
@ -450,8 +439,14 @@ void loop()
|
||||
if (millis() - medium_loopTimer > 19) {
|
||||
delta_ms_medium_loop = millis() - medium_loopTimer;
|
||||
medium_loopTimer = millis();
|
||||
|
||||
medium_loop();
|
||||
|
||||
counter_one_herz++;
|
||||
if(counter_one_herz == 50){
|
||||
super_slow_loop();
|
||||
}
|
||||
|
||||
if (millis() - perf_mon_timer > 20000) {
|
||||
if (mainLoop_count != 0) {
|
||||
gcs.send_message(MSG_PERF_REPORT);
|
||||
@ -541,15 +536,10 @@ void medium_loop()
|
||||
case 2:
|
||||
medium_loopCounter++;
|
||||
|
||||
|
||||
// Read altitude from sensors
|
||||
// ------------------
|
||||
update_alt();
|
||||
|
||||
// altitude smoothing
|
||||
// ------------------
|
||||
calc_altitude_error();
|
||||
|
||||
// perform next command
|
||||
// --------------------
|
||||
update_commands();
|
||||
@ -643,9 +633,6 @@ void slow_loop()
|
||||
}
|
||||
#endif
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_CUR)
|
||||
Log_Write_Current();
|
||||
|
||||
superslow_loopCounter = 0;
|
||||
}
|
||||
break;
|
||||
@ -690,6 +677,13 @@ void slow_loop()
|
||||
}
|
||||
}
|
||||
|
||||
void super_slow_loop()
|
||||
{
|
||||
if (g.log_bitmask & MASK_LOG_CUR)
|
||||
Log_Write_Current();
|
||||
}
|
||||
|
||||
|
||||
void update_GPS(void)
|
||||
{
|
||||
g_gps->update();
|
||||
@ -737,17 +731,6 @@ void update_GPS(void)
|
||||
current_loc.lng = g_gps->longitude; // Lon * 10 * *7
|
||||
current_loc.lat = g_gps->latitude; // Lat * 10 * *7
|
||||
|
||||
/*Serial.printf_P(PSTR("Lat: %.7f, Lon: %.7f, Alt: %dm, GSP: %d COG: %d, dist: %d, #sats: %d\n"),
|
||||
((float)g_gps->latitude / T7),
|
||||
((float)g_gps->longitude / T7),
|
||||
(int)g_gps->altitude / 100,
|
||||
(int)g_gps->ground_speed / 100,
|
||||
(int)g_gps->ground_course / 100,
|
||||
(int)wp_distance,
|
||||
(int)g_gps->num_sats);
|
||||
*/
|
||||
}else{
|
||||
//Serial.println("No fix");
|
||||
}
|
||||
}
|
||||
|
||||
@ -763,11 +746,6 @@ void update_current_flight_mode(void)
|
||||
// break;
|
||||
|
||||
default:
|
||||
|
||||
// based on altitude error
|
||||
// -----------------------
|
||||
calc_nav_throttle();
|
||||
|
||||
// Output Pitch, Roll, Yaw and Throttle
|
||||
// ------------------------------------
|
||||
auto_yaw();
|
||||
@ -906,10 +884,6 @@ void update_current_flight_mode(void)
|
||||
// -----------
|
||||
output_manual_yaw();
|
||||
|
||||
// based on altitude error
|
||||
// -----------------------
|
||||
calc_nav_throttle();
|
||||
|
||||
// Output Pitch, Roll, Yaw and Throttle
|
||||
// ------------------------------------
|
||||
// apply throttle control
|
||||
@ -924,10 +898,6 @@ void update_current_flight_mode(void)
|
||||
break;
|
||||
|
||||
case RTL:
|
||||
// based on altitude error
|
||||
// -----------------------
|
||||
calc_nav_throttle();
|
||||
|
||||
// Output Pitch, Roll, Yaw and Throttle
|
||||
// ------------------------------------
|
||||
auto_yaw();
|
||||
@ -949,11 +919,6 @@ void update_current_flight_mode(void)
|
||||
// -----------
|
||||
output_manual_yaw();
|
||||
|
||||
// based on altitude error
|
||||
// -----------------------
|
||||
calc_nav_throttle();
|
||||
|
||||
|
||||
// Output Pitch, Roll, Yaw and Throttle
|
||||
// ------------------------------------
|
||||
|
||||
@ -1031,6 +996,7 @@ void update_alt()
|
||||
{
|
||||
altitude_sensor = BARO;
|
||||
baro_alt = read_barometer();
|
||||
//Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt);
|
||||
|
||||
if(g.sonar_enabled){
|
||||
// decide which sensor we're usings
|
||||
@ -1058,4 +1024,13 @@ void update_alt()
|
||||
// no sonar altitude
|
||||
current_loc.alt = baro_alt + home.alt;
|
||||
}
|
||||
//Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt);
|
||||
|
||||
// altitude smoothing
|
||||
// ------------------
|
||||
calc_altitude_error();
|
||||
|
||||
// Amount of throttle to apply for hovering
|
||||
// ----------------------------------------
|
||||
calc_nav_throttle();
|
||||
}
|
||||
@ -129,15 +129,21 @@ output_yaw_with_hold(boolean hold)
|
||||
g.rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000
|
||||
|
||||
}else{
|
||||
|
||||
// rate control
|
||||
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
|
||||
rate = constrain(rate, -36000, 36000); // limit to something fun!
|
||||
|
||||
if(abs(rate) < 1000 ) //experiment to limit yaw reversing
|
||||
rate = 0;
|
||||
|
||||
long error = ((long)g.rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000
|
||||
// -error = CCW, +error = CW
|
||||
g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // kP .07 * 36000 = 2520
|
||||
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -2400, 2400); // limit to 24°
|
||||
}
|
||||
}
|
||||
// slight left rudder give right roll.
|
||||
|
||||
void
|
||||
output_rate_roll()
|
||||
@ -201,32 +207,24 @@ void calc_nav_throttle()
|
||||
{
|
||||
// limit error
|
||||
long error = constrain(altitude_error, -400, 400);
|
||||
float scaler = 1.0;
|
||||
|
||||
if(altitude_sensor == BARO) {
|
||||
float t = g.pid_baro_throttle.kP();
|
||||
if(error < 0){
|
||||
scaler = (altitude_sensor == BARO) ? .5 : .9;
|
||||
}
|
||||
|
||||
if(error > 0){ // go up
|
||||
g.pid_baro_throttle.kP(t);
|
||||
}else{ // go down
|
||||
g.pid_baro_throttle.kP(t/4.0);
|
||||
}
|
||||
|
||||
// limit output of throttle control
|
||||
nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
|
||||
if(altitude_sensor == BARO){
|
||||
nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, scaler);
|
||||
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80);
|
||||
|
||||
g.pid_baro_throttle.kP(t);
|
||||
|
||||
}else{
|
||||
// SONAR
|
||||
nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
|
||||
|
||||
// limit output of throttle control
|
||||
nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, scaler);
|
||||
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 100);
|
||||
}
|
||||
|
||||
nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
|
||||
nav_throttle_old = nav_throttle;
|
||||
|
||||
//Serial.printf("nav_thr %d, scaler %2.2f ", nav_throttle, scaler);
|
||||
}
|
||||
|
||||
float angle_boost()
|
||||
|
||||
@ -247,7 +247,7 @@ public:
|
||||
log_bitmask (0, k_param_log_bitmask, PSTR("LOG_BITMASK")),
|
||||
ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")),
|
||||
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")),
|
||||
RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),
|
||||
|
||||
// RC channel group key name
|
||||
//----------------------------------------------------------------------
|
||||
|
||||
@ -95,8 +95,7 @@ void decrement_WP_index()
|
||||
|
||||
long read_alt_to_hold()
|
||||
{
|
||||
read_EEPROM_alt_RTL();
|
||||
if(g.RTL_altitude == -1)
|
||||
if(g.RTL_altitude < 0)
|
||||
return current_loc.alt;
|
||||
else
|
||||
return g.RTL_altitude + home.alt;
|
||||
|
||||
@ -535,7 +535,7 @@
|
||||
#ifndef ALT_HOLD_HOME
|
||||
# define ALT_HOLD_HOME 8
|
||||
#endif
|
||||
#define ALT_HOLD_HOME_CM ALT_HOLD_HOME*100
|
||||
#define ALT_HOLD_HOME_CM ALT_HOLD_HOME * 100
|
||||
|
||||
#ifndef USE_CURRENT_ALT
|
||||
# define USE_CURRENT_ALT FALSE
|
||||
|
||||
@ -74,8 +74,8 @@ set_servos_4()
|
||||
|
||||
}else if(g.frame_type == X_FRAME){
|
||||
//Serial.println("X_FRAME");
|
||||
int roll_out = g.rc_1.pwm_out / 2;
|
||||
int pitch_out = g.rc_2.pwm_out / 2;
|
||||
int roll_out = g.rc_1.pwm_out * .707;
|
||||
int pitch_out = g.rc_2.pwm_out * .707;
|
||||
|
||||
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out;
|
||||
motor_out[CH_2] = g.rc_3.radio_out + roll_out - pitch_out;
|
||||
|
||||
@ -86,8 +86,10 @@ void calc_nav()
|
||||
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
|
||||
nav_pitch = -((float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y);
|
||||
|
||||
nav_roll = constrain(nav_roll, -g.pitch_max.get(), g.pitch_max.get());
|
||||
nav_pitch = constrain(nav_pitch, -g.pitch_max.get(), g.pitch_max.get());
|
||||
long pmax = g.pitch_max.get();
|
||||
|
||||
nav_roll = constrain(nav_roll, -pmax, pmax);
|
||||
nav_pitch = constrain(nav_pitch, -pmax, pmax);
|
||||
}
|
||||
|
||||
void calc_bearing_error()
|
||||
|
||||
@ -1,17 +1,28 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
//Function that will read the radio data, limit servos and trigger a failsafe
|
||||
// ----------------------------------------------------------------------------
|
||||
byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
|
||||
|
||||
|
||||
void init_rc_in()
|
||||
{
|
||||
read_EEPROM_radio(); // read Radio limits
|
||||
//read_EEPROM_radio(); // read Radio limits
|
||||
|
||||
// set rc channel ranges
|
||||
g.rc_1.set_angle(4500);
|
||||
g.rc_2.set_angle(4500);
|
||||
g.rc_3.set_range(0,1000);
|
||||
g.rc_3.scale_output = .9;
|
||||
g.rc_4.set_angle(6000);
|
||||
|
||||
// set rc dead zones
|
||||
g.rc_1.dead_zone = 60; // 60 = .6 degrees
|
||||
g.rc_2.dead_zone = 60;
|
||||
g.rc_3.dead_zone = 20;
|
||||
g.rc_4.dead_zone = 500;
|
||||
|
||||
//set auxiliary ranges
|
||||
g.rc_5.set_range(0,1000);
|
||||
g.rc_5.set_filter(false);
|
||||
g.rc_6.set_range(0,1000);
|
||||
@ -56,10 +67,12 @@ void read_radio()
|
||||
g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
|
||||
g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
|
||||
g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
|
||||
|
||||
//throttle_failsafe(g.rc_3.radio_in);
|
||||
|
||||
//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"), g.rc_1.control_in, g.rc_2.control_in, g.rc_3.control_in, g.rc_4.control_in);
|
||||
}
|
||||
|
||||
/*
|
||||
void throttle_failsafe(uint16_t pwm)
|
||||
{
|
||||
if(g.throttle_fs_enabled == 0)
|
||||
@ -101,7 +114,6 @@ void throttle_failsafe(uint16_t pwm)
|
||||
}
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
void trim_radio()
|
||||
{
|
||||
|
||||
@ -1,33 +1,75 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
// Sensors are not available in HIL_MODE_ATTITUDE
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
|
||||
void ReadSCP1000(void) {}
|
||||
|
||||
void init_barometer(void)
|
||||
{
|
||||
for(int i = 0; i < 300; i++){ // We take some readings...
|
||||
delay(20);
|
||||
barometer.Read(); // Get initial data from absolute pressure sensor
|
||||
ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l;
|
||||
ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10;
|
||||
}
|
||||
abs_pressure = barometer.Press;
|
||||
}
|
||||
int flashcount;
|
||||
|
||||
// Sensors are not available in HIL_MODE_ATTITUDE
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
#if HIL_MODE == HIL_MODE_SENSORS
|
||||
hil.update(); // look for inbound hil packets for initialization
|
||||
#endif
|
||||
|
||||
ground_pressure = 0;
|
||||
|
||||
while(ground_pressure == 0){
|
||||
barometer.Read(); // Get initial data from absolute pressure sensor
|
||||
ground_pressure = barometer.Press;
|
||||
ground_temperature = barometer.Temp;
|
||||
delay(20);
|
||||
Serial.printf("barometer.Press %ld\n", barometer.Press);
|
||||
}
|
||||
|
||||
for(int i = 0; i < 30; i++){ // We take some readings...
|
||||
|
||||
#if HIL_MODE == HIL_MODE_SENSORS
|
||||
hil.update(); // look for inbound hil packets
|
||||
#endif
|
||||
|
||||
barometer.Read(); // Get initial data from absolute pressure sensor
|
||||
ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l;
|
||||
ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10;
|
||||
|
||||
delay(20);
|
||||
if(flashcount == 5) {
|
||||
digitalWrite(C_LED_PIN, LOW);
|
||||
digitalWrite(A_LED_PIN, HIGH);
|
||||
digitalWrite(B_LED_PIN, LOW);
|
||||
}
|
||||
|
||||
if(flashcount >= 10) {
|
||||
flashcount = 0;
|
||||
digitalWrite(C_LED_PIN, HIGH);
|
||||
digitalWrite(A_LED_PIN, LOW);
|
||||
digitalWrite(B_LED_PIN, HIGH);
|
||||
}
|
||||
flashcount++;
|
||||
}
|
||||
|
||||
// makes the filtering work later
|
||||
abs_pressure = barometer.Press;
|
||||
|
||||
// save home pressure - will be overwritten by init_home, no big deal
|
||||
ground_pressure = abs_pressure;
|
||||
|
||||
//Serial.printf("abs_pressure %ld\n", abs_pressure);
|
||||
SendDebugln("barometer calibration complete.");
|
||||
}
|
||||
|
||||
long read_barometer(void)
|
||||
{
|
||||
float x, scaling, temp;
|
||||
|
||||
barometer.Read(); // Get new data from absolute pressure sensor
|
||||
barometer.Read(); // Get new data from absolute pressure sensor
|
||||
|
||||
//abs_pressure = (abs_pressure + barometer.Press) >> 1; // Small filtering
|
||||
abs_pressure = ((float)abs_pressure * .7) + ((float)barometer.Press * .3); // large filtering
|
||||
scaling = (float)ground_pressure / (float)abs_pressure;
|
||||
temp = ((float)ground_temperature / 10.0f) + 273.15f;
|
||||
x = log(scaling) * temp * 29271.267f;
|
||||
|
||||
return (x / 10);
|
||||
}
|
||||
|
||||
@ -37,10 +79,13 @@ void read_airspeed(void)
|
||||
|
||||
}
|
||||
|
||||
void zero_airspeed(void)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
#endif // HIL_MODE != HIL_MODE_ATTITUDE
|
||||
|
||||
|
||||
|
||||
#if BATTERY_EVENT == 1
|
||||
void read_battery(void)
|
||||
{
|
||||
|
||||
@ -33,8 +33,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
|
||||
{"compass", setup_compass},
|
||||
{"mag_offset", setup_mag_offset},
|
||||
{"declination", setup_declination},
|
||||
{"show", setup_show},
|
||||
{"ap_show", AP_Var_menu_show}
|
||||
{"show", setup_show}
|
||||
};
|
||||
|
||||
// Create the setup menu object.
|
||||
@ -75,6 +74,8 @@ setup_show(uint8_t argc, const Menu::arg *argv)
|
||||
report_flight_modes();
|
||||
report_imu();
|
||||
report_compass();
|
||||
|
||||
AP_Var_menu_show(argc, argv);
|
||||
return(0);
|
||||
}
|
||||
|
||||
@ -388,7 +389,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
// escape hatch
|
||||
if(Serial.available() > 0){
|
||||
save_EEPROM_flight_modes();
|
||||
g.flight_modes.save();
|
||||
print_done();
|
||||
report_flight_modes();
|
||||
return (0);
|
||||
@ -426,7 +427,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
|
||||
return 0;
|
||||
}
|
||||
|
||||
save_EEPROM_mag();
|
||||
g.compass_enabled.save();
|
||||
report_compass();
|
||||
return 0;
|
||||
}
|
||||
@ -451,8 +452,7 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
|
||||
report_frame();
|
||||
return 0;
|
||||
}
|
||||
|
||||
save_EEPROM_frame();
|
||||
g.frame_type.save();
|
||||
report_frame();
|
||||
return 0;
|
||||
}
|
||||
@ -508,7 +508,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
|
||||
compass.init(); // Initialization
|
||||
compass.set_orientation(MAGORIENTATION); // set compass's orientation on aircraft
|
||||
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
|
||||
//compass.set_offsets(0, 0, 0); // set offsets to account for surrounding interference
|
||||
//int counter = 0;
|
||||
float _min[3], _max[3], _offset[3];
|
||||
@ -600,15 +600,13 @@ default_nav()
|
||||
void
|
||||
default_alt_hold()
|
||||
{
|
||||
g.RTL_altitude.set(-1);
|
||||
save_EEPROM_alt_RTL();
|
||||
g.RTL_altitude.set_and_save(-1);
|
||||
}
|
||||
|
||||
void
|
||||
default_frame()
|
||||
{
|
||||
g.frame_type = PLUS_FRAME;
|
||||
save_EEPROM_frame();
|
||||
g.frame_type.set_and_save(PLUS_FRAME);
|
||||
}
|
||||
|
||||
void
|
||||
@ -628,7 +626,7 @@ default_flight_modes()
|
||||
g.flight_modes[3] = FLIGHT_MODE_4;
|
||||
g.flight_modes[4] = FLIGHT_MODE_5;
|
||||
g.flight_modes[5] = FLIGHT_MODE_6;
|
||||
save_EEPROM_flight_modes();
|
||||
g.flight_modes.save();
|
||||
}
|
||||
|
||||
void
|
||||
@ -661,7 +659,7 @@ void default_logs()
|
||||
LOGBIT(CURRENT);
|
||||
#undef LOGBIT
|
||||
|
||||
save_EEPROM_logs();
|
||||
g.log_bitmask.save();
|
||||
}
|
||||
|
||||
|
||||
@ -686,9 +684,7 @@ default_gains()
|
||||
|
||||
|
||||
// stabilize, angle error
|
||||
Serial.printf("b4 %4.2f, ",g.pid_stabilize_roll.kP());
|
||||
g.pid_stabilize_roll.kP(STABILIZE_ROLL_P);
|
||||
Serial.printf("L8R %4.2f\n ",g.pid_stabilize_roll.kP());
|
||||
g.pid_stabilize_roll.kI(STABILIZE_ROLL_I);
|
||||
g.pid_stabilize_roll.kD(0);
|
||||
g.pid_stabilize_roll.imax(STABILIZE_ROLL_IMAX * 100);
|
||||
@ -734,8 +730,6 @@ default_gains()
|
||||
g.pid_sonar_throttle.imax(THROTTLE_SONAR_IMAX);
|
||||
|
||||
save_EEPROM_PID();
|
||||
Serial.printf("EL8R %4.2f\n ",g.pid_stabilize_roll.kP());
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -746,7 +740,6 @@ default_gains()
|
||||
|
||||
void report_current()
|
||||
{
|
||||
//print_blanks(2);
|
||||
read_EEPROM_current();
|
||||
Serial.printf_P(PSTR("Current Sensor\n"));
|
||||
print_divider();
|
||||
@ -758,7 +751,6 @@ void report_current()
|
||||
|
||||
void report_sonar()
|
||||
{
|
||||
//print_blanks(2);
|
||||
g.sonar_enabled.load();
|
||||
Serial.printf_P(PSTR("Sonar Sensor\n"));
|
||||
print_divider();
|
||||
@ -769,8 +761,6 @@ void report_sonar()
|
||||
|
||||
void report_frame()
|
||||
{
|
||||
//print_blanks(2);
|
||||
read_EEPROM_frame();
|
||||
Serial.printf_P(PSTR("Frame\n"));
|
||||
print_divider();
|
||||
|
||||
@ -790,7 +780,6 @@ void report_frame()
|
||||
|
||||
void report_radio()
|
||||
{
|
||||
//print_blanks(2);
|
||||
Serial.printf_P(PSTR("Radio\n"));
|
||||
print_divider();
|
||||
// radio
|
||||
@ -801,7 +790,6 @@ void report_radio()
|
||||
|
||||
void report_gains()
|
||||
{
|
||||
//print_blanks(2);
|
||||
Serial.printf_P(PSTR("Gains\n"));
|
||||
print_divider();
|
||||
|
||||
@ -839,7 +827,6 @@ void report_gains()
|
||||
|
||||
void report_xtrack()
|
||||
{
|
||||
//print_blanks(2);
|
||||
Serial.printf_P(PSTR("Crosstrack\n"));
|
||||
print_divider();
|
||||
// radio
|
||||
@ -855,7 +842,6 @@ void report_xtrack()
|
||||
|
||||
void report_throttle()
|
||||
{
|
||||
//print_blanks(2);
|
||||
Serial.printf_P(PSTR("Throttle\n"));
|
||||
print_divider();
|
||||
|
||||
@ -875,7 +861,6 @@ void report_throttle()
|
||||
|
||||
void report_imu()
|
||||
{
|
||||
//print_blanks(2);
|
||||
Serial.printf_P(PSTR("IMU\n"));
|
||||
print_divider();
|
||||
|
||||
@ -886,14 +871,9 @@ void report_imu()
|
||||
|
||||
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(g.compass_enabled);
|
||||
|
||||
// mag declination
|
||||
@ -912,10 +892,8 @@ void report_compass()
|
||||
|
||||
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, g.flight_modes[i]);
|
||||
@ -1041,3 +1019,194 @@ print_gyro_offsets(void)
|
||||
}
|
||||
|
||||
|
||||
|
||||
/***************************************************************************/
|
||||
// EEPROM convenience functions
|
||||
/***************************************************************************/
|
||||
|
||||
|
||||
void read_EEPROM_waypoint_info(void)
|
||||
{
|
||||
g.waypoint_total.load();
|
||||
g.waypoint_radius.load();
|
||||
g.loiter_radius.load();
|
||||
}
|
||||
|
||||
void save_EEPROM_waypoint_info(void)
|
||||
{
|
||||
g.waypoint_total.save();
|
||||
g.waypoint_radius.save();
|
||||
g.loiter_radius.save();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
void read_EEPROM_nav(void)
|
||||
{
|
||||
g.crosstrack_gain.load();
|
||||
g.crosstrack_entry_angle.load();
|
||||
g.pitch_max.load();
|
||||
}
|
||||
|
||||
void save_EEPROM_nav(void)
|
||||
{
|
||||
g.crosstrack_gain.save();
|
||||
g.crosstrack_entry_angle.save();
|
||||
g.pitch_max.save();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
void read_EEPROM_PID(void)
|
||||
{
|
||||
g.pid_acro_rate_roll.load_gains();
|
||||
g.pid_acro_rate_pitch.load_gains();
|
||||
g.pid_acro_rate_yaw.load_gains();
|
||||
|
||||
g.pid_stabilize_roll.load_gains();
|
||||
g.pid_stabilize_pitch.load_gains();
|
||||
g.pid_yaw.load_gains();
|
||||
|
||||
g.pid_nav_lon.load_gains();
|
||||
g.pid_nav_lat.load_gains();
|
||||
g.pid_baro_throttle.load_gains();
|
||||
g.pid_sonar_throttle.load_gains();
|
||||
|
||||
// roll pitch
|
||||
g.stabilize_dampener.load();
|
||||
|
||||
// yaw
|
||||
g.hold_yaw_dampener.load();
|
||||
init_pids();
|
||||
}
|
||||
|
||||
void save_EEPROM_PID(void)
|
||||
{
|
||||
|
||||
g.pid_acro_rate_roll.save_gains();
|
||||
g.pid_acro_rate_pitch.save_gains();
|
||||
g.pid_acro_rate_yaw.save_gains();
|
||||
|
||||
g.pid_stabilize_roll.save_gains();
|
||||
g.pid_stabilize_pitch.save_gains();
|
||||
g.pid_yaw.save_gains();
|
||||
|
||||
g.pid_nav_lon.save_gains();
|
||||
g.pid_nav_lat.save_gains();
|
||||
g.pid_baro_throttle.save_gains();
|
||||
g.pid_sonar_throttle.save_gains();
|
||||
|
||||
// roll pitch
|
||||
g.stabilize_dampener.save();
|
||||
// yaw
|
||||
g.hold_yaw_dampener.save();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
void save_EEPROM_current(void)
|
||||
{
|
||||
g.current_enabled.save();
|
||||
g.milliamp_hours.save();
|
||||
}
|
||||
|
||||
void read_EEPROM_current(void)
|
||||
{
|
||||
g.current_enabled.load();
|
||||
g.milliamp_hours.load();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
void read_EEPROM_radio(void)
|
||||
{
|
||||
g.rc_1.load_eeprom();
|
||||
g.rc_2.load_eeprom();
|
||||
g.rc_3.load_eeprom();
|
||||
g.rc_4.load_eeprom();
|
||||
g.rc_5.load_eeprom();
|
||||
g.rc_6.load_eeprom();
|
||||
g.rc_7.load_eeprom();
|
||||
g.rc_8.load_eeprom();
|
||||
}
|
||||
|
||||
void save_EEPROM_radio(void)
|
||||
{
|
||||
g.rc_1.save_eeprom();
|
||||
g.rc_2.save_eeprom();
|
||||
g.rc_3.save_eeprom();
|
||||
g.rc_4.save_eeprom();
|
||||
g.rc_5.save_eeprom();
|
||||
g.rc_6.save_eeprom();
|
||||
g.rc_7.save_eeprom();
|
||||
g.rc_8.save_eeprom();
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
// configs are the basics
|
||||
void read_EEPROM_throttle(void)
|
||||
{
|
||||
g.throttle_min.load();
|
||||
g.throttle_max.load();
|
||||
g.throttle_cruise.load();
|
||||
g.throttle_fs_enabled.load();
|
||||
g.throttle_fs_action.load();
|
||||
g.throttle_fs_value.load();
|
||||
}
|
||||
|
||||
void save_EEPROM_throttle(void)
|
||||
{
|
||||
g.throttle_min.load();
|
||||
g.throttle_max.load();
|
||||
g.throttle_cruise.save();
|
||||
g.throttle_fs_enabled.load();
|
||||
g.throttle_fs_action.load();
|
||||
g.throttle_fs_value.load();
|
||||
}
|
||||
|
||||
|
||||
/********************************************************************************/
|
||||
/*
|
||||
float
|
||||
read_EE_float(int address)
|
||||
{
|
||||
union {
|
||||
byte bytes[4];
|
||||
float value;
|
||||
} _floatOut;
|
||||
|
||||
for (int i = 0; i < 4; i++)
|
||||
_floatOut.bytes[i] = eeprom_read_byte((uint8_t *) (address + i));
|
||||
return _floatOut.value;
|
||||
}
|
||||
|
||||
void write_EE_float(float value, int address)
|
||||
{
|
||||
union {
|
||||
byte bytes[4];
|
||||
float value;
|
||||
} _floatIn;
|
||||
|
||||
_floatIn.value = value;
|
||||
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)
|
||||
{
|
||||
float scale = pow(10, places);
|
||||
|
||||
int temp = eeprom_read_word((uint16_t *) address);
|
||||
return ((float)temp / scale);
|
||||
}
|
||||
|
||||
void write_EE_compressed_float(float value, int address, byte places)
|
||||
{
|
||||
float scale = pow(10, places);
|
||||
int temp = value * scale;
|
||||
eeprom_write_word((uint16_t *) address, temp);
|
||||
}
|
||||
*/
|
||||
@ -392,7 +392,7 @@ init_compass()
|
||||
{
|
||||
dcm.set_compass(&compass);
|
||||
bool junkbool = compass.init();
|
||||
compass.set_orientation(MAGORIENTATION); // set compass's orientation on aircraft
|
||||
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
|
||||
Vector3f junkvector = compass.get_offsets(); // load offsets to account for airframe magnetic interference
|
||||
}
|
||||
|
||||
|
||||
@ -351,6 +351,8 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
|
||||
// IMU
|
||||
// ---
|
||||
read_AHRS();
|
||||
update_trig();
|
||||
|
||||
|
||||
// allow us to zero out sensors with control switches
|
||||
if(g.rc_5.control_in < 600){
|
||||
@ -367,6 +369,7 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
ts_num++;
|
||||
if (ts_num == 5){
|
||||
update_alt();
|
||||
// 10 hz
|
||||
ts_num = 0;
|
||||
g_gps->longitude = 0;
|
||||
@ -474,7 +477,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
|
||||
accels.x, accels.y, accels.z,
|
||||
gyros.x, gyros.y, gyros.z);
|
||||
*/
|
||||
Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld\t"),
|
||||
Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld\n"),
|
||||
dcm.roll_sensor,
|
||||
dcm.pitch_sensor,
|
||||
dcm.yaw_sensor);
|
||||
@ -717,10 +720,10 @@ test_wp(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
|
||||
// save the alitude above home option
|
||||
if(g.RTL_altitude == -1){
|
||||
if(g.RTL_altitude < 0){
|
||||
Serial.printf_P(PSTR("Hold current altitude\n"));
|
||||
}else{
|
||||
Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude);
|
||||
Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude / 100);
|
||||
}
|
||||
|
||||
Serial.printf_P(PSTR("%d waypoints\n"), (int)g.waypoint_total);
|
||||
@ -769,39 +772,25 @@ test_xbee(uint8_t argc, const Menu::arg *argv)
|
||||
static int8_t
|
||||
test_pressure(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
uint32_t sum;
|
||||
|
||||
Serial.printf_P(PSTR("Uncalibrated Abs Airpressure\n"));
|
||||
Serial.printf_P(PSTR("Altitude is relative to the start of this test\n"));
|
||||
Serial.printf_P(PSTR("Uncalibrated relative airpressure\n"));
|
||||
print_hit_enter();
|
||||
|
||||
Serial.printf_P(PSTR("\nCalibrating....\n"));
|
||||
/*
|
||||
for (int i = 1; i < 301; i++) {
|
||||
baro_alt = read_barometer();
|
||||
if(i > 200)
|
||||
sum += abs_pressure;
|
||||
delay(10);
|
||||
}
|
||||
ground_pressure = (float)sum / 100.0;
|
||||
*/
|
||||
|
||||
home.alt = 0;
|
||||
wp_distance = 0;
|
||||
init_barometer();
|
||||
reset_I();
|
||||
|
||||
// to prevent boost from skewing results
|
||||
cos_pitch_x = cos_roll_x = 1;
|
||||
|
||||
while(1){
|
||||
if (millis()-fast_loopTimer > 9) {
|
||||
if (millis() - fast_loopTimer > 100) {
|
||||
delta_ms_fast_loop = millis() - fast_loopTimer;
|
||||
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
|
||||
fast_loopTimer = millis();
|
||||
|
||||
|
||||
calc_altitude_error();
|
||||
calc_nav_throttle();
|
||||
}
|
||||
|
||||
if (millis()-medium_loopTimer > 100) {
|
||||
if (millis() - medium_loopTimer > 100) {
|
||||
medium_loopTimer = millis();
|
||||
|
||||
read_radio(); // read the radio first
|
||||
@ -809,14 +798,15 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
|
||||
next_WP.alt = max(next_WP.alt, 30);
|
||||
|
||||
read_trim_switch();
|
||||
baro_alt = read_barometer();
|
||||
current_loc.alt = baro_alt + home.alt;
|
||||
Serial.printf_P(PSTR("AP: %ld,\tAlt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \t out:%d\n"),
|
||||
abs_pressure,
|
||||
update_alt();
|
||||
output_auto_throttle();
|
||||
|
||||
Serial.printf_P(PSTR("Alt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \tint: %6.2f \tout:%d\n"),
|
||||
baro_alt,
|
||||
next_WP.alt,
|
||||
altitude_error,
|
||||
(int)g.throttle_cruise,
|
||||
g.pid_baro_throttle.get_integrator(),
|
||||
g.rc_3.servo_out);
|
||||
|
||||
/*
|
||||
@ -843,29 +833,35 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
static int8_t
|
||||
test_mag(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
if(g.compass_enabled) {
|
||||
print_hit_enter();
|
||||
{
|
||||
if(g.compass_enabled) {
|
||||
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
|
||||
|
||||
while(1){
|
||||
delay(250);
|
||||
compass.read();
|
||||
compass.calculate(0,0);
|
||||
Serial.printf_P(PSTR("Heading: %4.2f, XYZ: %4.2f, %4.2f, %4.2f"),
|
||||
ToDeg(compass.heading),
|
||||
compass.mag_x,
|
||||
compass.mag_y,
|
||||
compass.mag_z);
|
||||
print_hit_enter();
|
||||
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
while(1){
|
||||
delay(250);
|
||||
compass.read();
|
||||
compass.calculate(0,0);
|
||||
Vector3f maggy = compass.get_offsets();
|
||||
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
|
||||
(wrap_360(ToDeg(compass.heading) * 100)) /100,
|
||||
compass.mag_x,
|
||||
compass.mag_y,
|
||||
compass.mag_z,
|
||||
maggy.x,
|
||||
maggy.y,
|
||||
maggy.z);
|
||||
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
Serial.printf_P(PSTR("Compass: "));
|
||||
print_enabled(false);
|
||||
return (0);
|
||||
}
|
||||
} else {
|
||||
Serial.printf_P(PSTR("Compass: "));
|
||||
print_enabled(false);
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
|
||||
void print_hit_enter()
|
||||
|
||||
Loading…
Reference in New Issue
Block a user