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

Should be flyable now. Please Use Reset in setup before flying.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1726 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-02-25 05:33:39 +00:00
parent 5d81f37b37
commit 957e1a747b
13 changed files with 459 additions and 274 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduCopterMega/APM_Config.h
View File

@ -6,11 +6,9 @@
#define GCS_PROTOCOL GCS_PROTOCOL_NONE #define GCS_PROTOCOL GCS_PROTOCOL_NONE
//#define MAGORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK //#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK
#define MAGORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
# define SERIAL0_BAUD 38400 # define SERIAL0_BAUD 38400
//# define SERIAL0_BAUD 115200
//# define STABILIZE_ROLL_P 0.4 //# define STABILIZE_ROLL_P 0.4
//# define STABILIZE_PITCH_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 // Logging
//#define LOG_CURRENT ENABLED //#define LOG_CURRENT ENABLED

253
ArduCopterMega/ArduCopterMega.pde
View File

@ -91,43 +91,52 @@ GPS *g_gps;
#if HIL_MODE == HIL_MODE_NONE #if HIL_MODE == HIL_MODE_NONE
// real sensors // real sensors
AP_ADC_ADS7844 adc; AP_ADC_ADS7844 adc;
APM_BMP085_Class barometer; APM_BMP085_Class barometer;
AP_Compass_HMC5843 compass(Parameters::k_param_compass); AP_Compass_HMC5843 compass(Parameters::k_param_compass);
// real GPS selection // real GPS selection
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps); 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_NMEA
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF AP_GPS_NMEA g_gps_driver(&Serial1);
AP_GPS_SIRF g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX #elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
AP_GPS_UBLOX g_gps_driver(&Serial1); AP_GPS_SIRF g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
AP_GPS_MTK g_gps_driver(&Serial1); #elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16 AP_GPS_UBLOX g_gps_driver(&Serial1);
AP_GPS_MTK16 g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
AP_GPS_None g_gps_driver(NULL); AP_GPS_MTK g_gps_driver(&Serial1);
#else
#error Unrecognised GPS_PROTOCOL setting. #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
#endif // GPS PROTOCOL 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 #elif HIL_MODE == HIL_MODE_SENSORS
// sensor emulators // sensor emulators
AP_ADC_HIL adc; AP_ADC_HIL adc;
APM_BMP085_HIL_Class barometer; APM_BMP085_HIL_Class barometer;
AP_Compass_HIL compass; AP_Compass_HIL compass;
AP_GPS_HIL g_gps_driver(NULL); AP_GPS_HIL g_gps_driver(NULL);
#elif HIL_MODE == HIL_MODE_ATTITUDE #elif HIL_MODE == HIL_MODE_ATTITUDE
AP_DCM_HIL dcm; AP_DCM_HIL dcm;
AP_GPS_HIL g_gps_driver(NULL); AP_GPS_HIL g_gps_driver(NULL);
AP_Compass_HIL compass; // never used
AP_IMU_Shim imu; // never used
#else #else
#error Unrecognised HIL_MODE setting. #error Unrecognised HIL_MODE setting.
#endif // HIL MODE #endif // HIL MODE
#if HIL_MODE != HIL_MODE_DISABLED #if HIL_MODE != HIL_MODE_DISABLED
@ -152,10 +161,10 @@ AP_GPS_HIL g_gps_driver(NULL);
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// //
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
GCS_MAVLINK gcs; GCS_MAVLINK gcs;
#else #else
// If we are not using a GCS, we need a stub that does nothing. // If we are not using a GCS, we need a stub that does nothing.
GCS_Class gcs; GCS_Class gcs;
#endif #endif
AP_RangeFinder_MaxsonarXL sonar; AP_RangeFinder_MaxsonarXL sonar;
@ -165,8 +174,6 @@ AP_RangeFinder_MaxsonarXL sonar;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
byte control_mode = STABILIZE; 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 byte oldSwitchPosition; // for remembering the control mode switch
const char *comma = ","; const char *comma = ",";
@ -177,6 +184,7 @@ const char* flight_mode_strings[] = {
"ALT_HOLD", "ALT_HOLD",
"FBW", "FBW",
"AUTO", "AUTO",
"LOITER",
"POSITION_HOLD", "POSITION_HOLD",
"RTL", "RTL",
"TAKEOFF", "TAKEOFF",
@ -189,7 +197,7 @@ const char* flight_mode_strings[] = {
3 Throttle 3 Throttle
4 Rudder (if we have ailerons) 4 Rudder (if we have ailerons)
5 Mode - 3 position switch 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) 7 trainer switch - sets throttle nominal (toggle switch), sets accels to Level (hold > 1 second)
8 TBD 8 TBD
*/ */
@ -199,7 +207,15 @@ const char* flight_mode_strings[] = {
int motor_out[4]; int motor_out[4];
Vector3f omega; Vector3f omega;
// Failsafe
// --------
boolean failsafe; // did our throttle dip below the failsafe value?
boolean ch3_failsafe;
boolean motor_armed;
boolean motor_auto_safe;
// PIDs // PIDs
// ----
int max_stabilize_dampener; // int max_stabilize_dampener; //
int max_yaw_dampener; // int max_yaw_dampener; //
boolean rate_yaw_flag; // used to transition yaw control from Rate control to Yaw hold 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 motor_light; // status of the Motor safety
boolean GPS_light; // status of the GPS light 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 // GPS variables
// ------------- // -------------
const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage 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 float scaleLongDown = 1; // used to reverse longtitude scaling
byte ground_start_count = 5; // have we achieved first lock and set Home? byte ground_start_count = 5; // have we achieved first lock and set Home?
// Magnetometer variables
// ----------------------
Vector3f mag_offsets;
// Location & Navigation // Location & Navigation
// --------------------- // ---------------------
const float radius_of_earth = 6378100; // meters const float radius_of_earth = 6378100; // meters
const float gravity = 9.81; // meters/ sec^2 const float gravity = 9.81; // meters/ sec^2
//byte wp_radius = 3; // meters
long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate 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 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 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 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_must_index; // current command memory location
byte command_may_index; // current command memory location byte command_may_index; // current command memory location
byte command_must_ID; // current command ID byte command_must_ID; // current command ID
byte command_may_ID; // current command ID byte command_may_ID; // current command ID
float cos_roll_x, sin_roll_y; float cos_roll_x = 1;
float cos_pitch_x, sin_pitch_y; float cos_pitch_x = 1;
float sin_yaw_y, cos_yaw_x; float cos_yaw_x = 1;
float sin_pitch_y, sin_yaw_y, sin_roll_y;
// Airspeed // Airspeed
// -------- // --------
int airspeed; // m/s * 100 int airspeed; // m/s * 100
float airspeed_error; // m / s * 100
// Throttle Failsafe
// ------------------
boolean motor_armed = false;
boolean motor_auto_safe = false;
// Location Errors // Location Errors
// --------------- // ---------------
long bearing_error; // deg * 100 : 0 to 36000 long bearing_error; // deg * 100 : 0 to 36000
long altitude_error; // meters * 100 we are off in altitude long altitude_error; // meters * 100 we are off in altitude
float crosstrack_error; // meters we are off trackline float crosstrack_error; // meters we are off trackline
long distance_error; // distance to the WP long distance_error; // distance to the WP
long yaw_error; // how off are we pointed long yaw_error; // how off are we pointed
long long_error, lat_error; // temp for debugging
long long_error, lat_error; // temp fro debugging // Battery Sensors
// ---------------
// Sensors
// -------
float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter 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_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 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_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
float current_amps; float current_amps;
float current_total; float current_total;
//int milliamp_hours;
//boolean current_enabled = false;
// Magnetometer variables // Airspeed Sensors
// ---------------------- // ----------------
//int magnetom_x;
//int magnetom_y;
//int magnetom_z;
//float mag_offset_x;
//float mag_offset_y;
//float mag_offset_z;
//float mag_declination;
// Barometer Sensor variables // Barometer Sensor variables
// -------------------------- // --------------------------
@ -292,20 +286,18 @@ unsigned long abs_pressure;
unsigned long ground_pressure; unsigned long ground_pressure;
int ground_temperature; int ground_temperature;
byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR // Altitude Sensor variables
// Sonar Sensor variables
// ---------------------- // ----------------------
long sonar_alt; long sonar_alt;
long baro_alt; long baro_alt;
byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
// flight mode specific // flight mode specific
// -------------------- // --------------------
boolean takeoff_complete = false; // Flag for using take-off controls boolean takeoff_complete; // Flag for using take-off controls
boolean land_complete = false; boolean land_complete;
int takeoff_altitude; int takeoff_altitude;
int landing_distance; // meters; int landing_distance; // meters;
long old_alt; // used for managing altitude rates long old_alt; // used for managing altitude rates
int velocity_land; 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 // these are the values for navigation control functions
// ---------------------------------------------------- // ----------------------------------------------------
long nav_roll; // deg * 100 : target roll angle long nav_roll; // deg * 100 : target roll angle
long nav_pitch; // deg * 100 : target pitch angle long nav_pitch; // deg * 100 : target pitch angle
long nav_yaw; // deg * 100 : target yaw angle long nav_yaw; // deg * 100 : target yaw angle
long nav_lat; // for error calcs long nav_lat; // for error calcs
long nav_lon; // for error calcs long nav_lon; // for error calcs
int nav_throttle; // 0-1000 for throttle control int nav_throttle; // 0-1000 for throttle control
int nav_throttle_old; // for filtering int nav_throttle_old; // for filtering
long command_yaw_start; // what angle were we to begin with long command_yaw_start; // what angle were we to begin with
long command_yaw_start_time; // when did we start turning long command_yaw_start_time; // when did we start turning
@ -338,12 +330,9 @@ byte command_yaw_dir;
// Waypoints // Waypoints
// --------- // ---------
//long GPS_wp_distance; // meters - distance between plane and next waypoint long 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
long wp_totalDistance; // meters - distance between old and next waypoint byte next_wp_index; // Current active command index
//byte wp_total; // # of Commands total including way
//byte wp_index; // Current active command index
byte next_wp_index; // Current active command index
// repeating event control // 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 // 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 // Performance monitoring
// ---------------------- // ----------------------
long perf_mon_timer; long perf_mon_timer;
float imu_health; // Metric based on accel gain deweighting float imu_health; // Metric based on accel gain deweighting
int G_Dt_max; // Max main loop cycle time in milliseconds int G_Dt_max; // Max main loop cycle time in milliseconds
byte gyro_sat_count; byte gyro_sat_count;
byte adc_constraints; byte adc_constraints;
byte renorm_sqrt_count; byte renorm_sqrt_count;
@ -395,7 +384,7 @@ byte gcs_messages_sent;
// GCS // GCS
// --- // ---
char GCS_buffer[53]; 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 // 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 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 nav2_loopTimer; // used to track the elapsed ime for GPS nav
unsigned long dTnav; // Delta Time in milliseconds for navigation computations unsigned long dTnav; // Delta Time in milliseconds for navigation computations
unsigned long dTnav2; // Delta Time in milliseconds for navigation computations unsigned long dTnav2; // Delta Time in milliseconds for navigation computations
unsigned long elapsedTime; // for doing custom events unsigned long elapsedTime; // for doing custom events
float load; // % MCU cycles used float load; // % MCU cycles used
byte counter_one_herz;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Top-level logic // Top-level logic
@ -450,8 +439,14 @@ void loop()
if (millis() - medium_loopTimer > 19) { if (millis() - medium_loopTimer > 19) {
delta_ms_medium_loop = millis() - medium_loopTimer; delta_ms_medium_loop = millis() - medium_loopTimer;
medium_loopTimer = millis(); medium_loopTimer = millis();
medium_loop(); medium_loop();
counter_one_herz++;
if(counter_one_herz == 50){
super_slow_loop();
}
if (millis() - perf_mon_timer > 20000) { if (millis() - perf_mon_timer > 20000) {
if (mainLoop_count != 0) { if (mainLoop_count != 0) {
gcs.send_message(MSG_PERF_REPORT); gcs.send_message(MSG_PERF_REPORT);
@ -541,15 +536,10 @@ void medium_loop()
case 2: case 2:
medium_loopCounter++; medium_loopCounter++;
// Read altitude from sensors // Read altitude from sensors
// ------------------ // ------------------
update_alt(); update_alt();
// altitude smoothing
// ------------------
calc_altitude_error();
// perform next command // perform next command
// -------------------- // --------------------
update_commands(); update_commands();
@ -643,9 +633,6 @@ void slow_loop()
} }
#endif #endif
if (g.log_bitmask & MASK_LOG_CUR)
Log_Write_Current();
superslow_loopCounter = 0; superslow_loopCounter = 0;
} }
break; 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) void update_GPS(void)
{ {
g_gps->update(); g_gps->update();
@ -737,17 +731,6 @@ void update_GPS(void)
current_loc.lng = g_gps->longitude; // Lon * 10 * *7 current_loc.lng = g_gps->longitude; // Lon * 10 * *7
current_loc.lat = g_gps->latitude; // Lat * 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; // break;
default: default:
// based on altitude error
// -----------------------
calc_nav_throttle();
// Output Pitch, Roll, Yaw and Throttle // Output Pitch, Roll, Yaw and Throttle
// ------------------------------------ // ------------------------------------
auto_yaw(); auto_yaw();
@ -906,10 +884,6 @@ void update_current_flight_mode(void)
// ----------- // -----------
output_manual_yaw(); output_manual_yaw();
// based on altitude error
// -----------------------
calc_nav_throttle();
// Output Pitch, Roll, Yaw and Throttle // Output Pitch, Roll, Yaw and Throttle
// ------------------------------------ // ------------------------------------
// apply throttle control // apply throttle control
@ -924,10 +898,6 @@ void update_current_flight_mode(void)
break; break;
case RTL: case RTL:
// based on altitude error
// -----------------------
calc_nav_throttle();
// Output Pitch, Roll, Yaw and Throttle // Output Pitch, Roll, Yaw and Throttle
// ------------------------------------ // ------------------------------------
auto_yaw(); auto_yaw();
@ -949,11 +919,6 @@ void update_current_flight_mode(void)
// ----------- // -----------
output_manual_yaw(); output_manual_yaw();
// based on altitude error
// -----------------------
calc_nav_throttle();
// Output Pitch, Roll, Yaw and Throttle // Output Pitch, Roll, Yaw and Throttle
// ------------------------------------ // ------------------------------------
@ -1031,6 +996,7 @@ void update_alt()
{ {
altitude_sensor = BARO; altitude_sensor = BARO;
baro_alt = read_barometer(); baro_alt = read_barometer();
//Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt);
if(g.sonar_enabled){ if(g.sonar_enabled){
// decide which sensor we're usings // decide which sensor we're usings
@ -1058,4 +1024,13 @@ void update_alt()
// no sonar altitude // no sonar altitude
current_loc.alt = baro_alt + home.alt; 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();
} }

32
ArduCopterMega/Attitude.pde
View File

@ -129,15 +129,21 @@ output_yaw_with_hold(boolean hold)
g.rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000 g.rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000
}else{ }else{
// rate control // rate control
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377 long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun! 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 long error = ((long)g.rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000
// -error = CCW, +error = CW // -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 = 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° g.rc_4.servo_out = constrain(g.rc_4.servo_out, -2400, 2400); // limit to 24°
} }
} }
// slight left rudder give right roll.
void void
output_rate_roll() output_rate_roll()
@ -201,32 +207,24 @@ void calc_nav_throttle()
{ {
// limit error // limit error
long error = constrain(altitude_error, -400, 400); long error = constrain(altitude_error, -400, 400);
float scaler = 1.0;
if(altitude_sensor == BARO) { if(error < 0){
float t = g.pid_baro_throttle.kP(); scaler = (altitude_sensor == BARO) ? .5 : .9;
}
if(error > 0){ // go up if(altitude_sensor == BARO){
g.pid_baro_throttle.kP(t); nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, scaler);
}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);
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80); nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80);
g.pid_baro_throttle.kP(t);
}else{ }else{
// SONAR nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, scaler);
nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
// limit output of throttle control
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 100); nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 100);
} }
nav_throttle = (nav_throttle + nav_throttle_old) >> 1; nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
nav_throttle_old = nav_throttle; nav_throttle_old = nav_throttle;
//Serial.printf("nav_thr %d, scaler %2.2f ", nav_throttle, scaler);
} }
float angle_boost() float angle_boost()

2
ArduCopterMega/Parameters.h
View File

@ -247,7 +247,7 @@ public:
log_bitmask (0, k_param_log_bitmask, PSTR("LOG_BITMASK")), log_bitmask (0, k_param_log_bitmask, PSTR("LOG_BITMASK")),
ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")), ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")),
ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")), 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 // RC channel group key name
//---------------------------------------------------------------------- //----------------------------------------------------------------------

3
ArduCopterMega/commands.pde
View File

@ -95,8 +95,7 @@ void decrement_WP_index()
long read_alt_to_hold() long read_alt_to_hold()
{ {
read_EEPROM_alt_RTL(); if(g.RTL_altitude < 0)
if(g.RTL_altitude == -1)
return current_loc.alt; return current_loc.alt;
else else
return g.RTL_altitude + home.alt; return g.RTL_altitude + home.alt;

2
ArduCopterMega/config.h
View File

@ -535,7 +535,7 @@
#ifndef ALT_HOLD_HOME #ifndef ALT_HOLD_HOME
# define ALT_HOLD_HOME 8 # define ALT_HOLD_HOME 8
#endif #endif
#define ALT_HOLD_HOME_CM ALT_HOLD_HOME*100 #define ALT_HOLD_HOME_CM ALT_HOLD_HOME * 100
#ifndef USE_CURRENT_ALT #ifndef USE_CURRENT_ALT
# define USE_CURRENT_ALT FALSE # define USE_CURRENT_ALT FALSE

4
ArduCopterMega/motors.pde
View File

@ -74,8 +74,8 @@ set_servos_4()
}else if(g.frame_type == X_FRAME){ }else if(g.frame_type == X_FRAME){
//Serial.println("X_FRAME"); //Serial.println("X_FRAME");
int roll_out = g.rc_1.pwm_out / 2; int roll_out = g.rc_1.pwm_out * .707;
int pitch_out = g.rc_2.pwm_out / 2; 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_3] = g.rc_3.radio_out + roll_out + pitch_out;
motor_out[CH_2] = g.rc_3.radio_out + roll_out - pitch_out; motor_out[CH_2] = g.rc_3.radio_out + roll_out - pitch_out;

6
ArduCopterMega/navigation.pde
View File

@ -86,8 +86,10 @@ void calc_nav()
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x; 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_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()); long pmax = g.pitch_max.get();
nav_pitch = constrain(nav_pitch, -g.pitch_max.get(), g.pitch_max.get());
nav_roll = constrain(nav_roll, -pmax, pmax);
nav_pitch = constrain(nav_pitch, -pmax, pmax);
} }
void calc_bearing_error() void calc_bearing_error()

18
ArduCopterMega/radio.pde
View File

@ -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() 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_1.set_angle(4500);
g.rc_2.set_angle(4500); g.rc_2.set_angle(4500);
g.rc_3.set_range(0,1000); g.rc_3.set_range(0,1000);
g.rc_3.scale_output = .9; g.rc_3.scale_output = .9;
g.rc_4.set_angle(6000); g.rc_4.set_angle(6000);
// set rc dead zones
g.rc_1.dead_zone = 60; // 60 = .6 degrees g.rc_1.dead_zone = 60; // 60 = .6 degrees
g.rc_2.dead_zone = 60; g.rc_2.dead_zone = 60;
g.rc_3.dead_zone = 20; g.rc_3.dead_zone = 20;
g.rc_4.dead_zone = 500; g.rc_4.dead_zone = 500;
//set auxiliary ranges
g.rc_5.set_range(0,1000); g.rc_5.set_range(0,1000);
g.rc_5.set_filter(false); g.rc_5.set_filter(false);
g.rc_6.set_range(0,1000); 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_6.set_pwm(APM_RC.InputCh(CH_6));
g.rc_7.set_pwm(APM_RC.InputCh(CH_7)); g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
g.rc_8.set_pwm(APM_RC.InputCh(CH_8)); 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); //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) void throttle_failsafe(uint16_t pwm)
{ {
if(g.throttle_fs_enabled == 0) if(g.throttle_fs_enabled == 0)
@ -101,7 +114,6 @@ void throttle_failsafe(uint16_t pwm)
} }
} }
} }
*/
void trim_radio() void trim_radio()
{ {

73
ArduCopterMega/sensors.pde
View File

@ -1,33 +1,75 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- 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 ReadSCP1000(void) {}
void init_barometer(void) void init_barometer(void)
{ {
for(int i = 0; i < 300; i++){ // We take some readings... int flashcount;
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;
}
// Sensors are not available in HIL_MODE_ATTITUDE #if HIL_MODE == HIL_MODE_SENSORS
#if HIL_MODE != HIL_MODE_ATTITUDE 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) long read_barometer(void)
{ {
float x, scaling, temp; 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 = (abs_pressure + barometer.Press) >> 1; // Small filtering
abs_pressure = ((float)abs_pressure * .7) + ((float)barometer.Press * .3); // large filtering abs_pressure = ((float)abs_pressure * .7) + ((float)barometer.Press * .3); // large filtering
scaling = (float)ground_pressure / (float)abs_pressure; scaling = (float)ground_pressure / (float)abs_pressure;
temp = ((float)ground_temperature / 10.0f) + 273.15f; temp = ((float)ground_temperature / 10.0f) + 273.15f;
x = log(scaling) * temp * 29271.267f; x = log(scaling) * temp * 29271.267f;
return (x / 10); return (x / 10);
} }
@ -37,10 +79,13 @@ void read_airspeed(void)
} }
void zero_airspeed(void)
{
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE #endif // HIL_MODE != HIL_MODE_ATTITUDE
#if BATTERY_EVENT == 1 #if BATTERY_EVENT == 1
void read_battery(void) void read_battery(void)
{ {

235
ArduCopterMega/setup.pde
View File

@ -33,8 +33,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
{"compass", setup_compass}, {"compass", setup_compass},
{"mag_offset", setup_mag_offset}, {"mag_offset", setup_mag_offset},
{"declination", setup_declination}, {"declination", setup_declination},
{"show", setup_show}, {"show", setup_show}
{"ap_show", AP_Var_menu_show}
}; };
// Create the setup menu object. // Create the setup menu object.
@ -75,6 +74,8 @@ setup_show(uint8_t argc, const Menu::arg *argv)
report_flight_modes(); report_flight_modes();
report_imu(); report_imu();
report_compass(); report_compass();
AP_Var_menu_show(argc, argv);
return(0); return(0);
} }
@ -388,7 +389,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
// escape hatch // escape hatch
if(Serial.available() > 0){ if(Serial.available() > 0){
save_EEPROM_flight_modes(); g.flight_modes.save();
print_done(); print_done();
report_flight_modes(); report_flight_modes();
return (0); return (0);
@ -426,7 +427,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
return 0; return 0;
} }
save_EEPROM_mag(); g.compass_enabled.save();
report_compass(); report_compass();
return 0; return 0;
} }
@ -451,8 +452,7 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
report_frame(); report_frame();
return 0; return 0;
} }
g.frame_type.save();
save_EEPROM_frame();
report_frame(); report_frame();
return 0; return 0;
} }
@ -508,7 +508,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
compass.init(); // Initialization 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 //compass.set_offsets(0, 0, 0); // set offsets to account for surrounding interference
//int counter = 0; //int counter = 0;
float _min[3], _max[3], _offset[3]; float _min[3], _max[3], _offset[3];
@ -600,15 +600,13 @@ default_nav()
void void
default_alt_hold() default_alt_hold()
{ {
g.RTL_altitude.set(-1); g.RTL_altitude.set_and_save(-1);
save_EEPROM_alt_RTL();
} }
void void
default_frame() default_frame()
{ {
g.frame_type = PLUS_FRAME; g.frame_type.set_and_save(PLUS_FRAME);
save_EEPROM_frame();
} }
void void
@ -628,7 +626,7 @@ default_flight_modes()
g.flight_modes[3] = FLIGHT_MODE_4; g.flight_modes[3] = FLIGHT_MODE_4;
g.flight_modes[4] = FLIGHT_MODE_5; g.flight_modes[4] = FLIGHT_MODE_5;
g.flight_modes[5] = FLIGHT_MODE_6; g.flight_modes[5] = FLIGHT_MODE_6;
save_EEPROM_flight_modes(); g.flight_modes.save();
} }
void void
@ -661,7 +659,7 @@ void default_logs()
LOGBIT(CURRENT); LOGBIT(CURRENT);
#undef LOGBIT #undef LOGBIT
save_EEPROM_logs(); g.log_bitmask.save();
} }
@ -686,9 +684,7 @@ default_gains()
// stabilize, angle error // stabilize, angle error
Serial.printf("b4 %4.2f, ",g.pid_stabilize_roll.kP());
g.pid_stabilize_roll.kP(STABILIZE_ROLL_P); 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.kI(STABILIZE_ROLL_I);
g.pid_stabilize_roll.kD(0); g.pid_stabilize_roll.kD(0);
g.pid_stabilize_roll.imax(STABILIZE_ROLL_IMAX * 100); g.pid_stabilize_roll.imax(STABILIZE_ROLL_IMAX * 100);
@ -734,8 +730,6 @@ default_gains()
g.pid_sonar_throttle.imax(THROTTLE_SONAR_IMAX); g.pid_sonar_throttle.imax(THROTTLE_SONAR_IMAX);
save_EEPROM_PID(); save_EEPROM_PID();
Serial.printf("EL8R %4.2f\n ",g.pid_stabilize_roll.kP());
} }
@ -746,7 +740,6 @@ default_gains()
void report_current() void report_current()
{ {
//print_blanks(2);
read_EEPROM_current(); read_EEPROM_current();
Serial.printf_P(PSTR("Current Sensor\n")); Serial.printf_P(PSTR("Current Sensor\n"));
print_divider(); print_divider();
@ -758,7 +751,6 @@ void report_current()
void report_sonar() void report_sonar()
{ {
//print_blanks(2);
g.sonar_enabled.load(); g.sonar_enabled.load();
Serial.printf_P(PSTR("Sonar Sensor\n")); Serial.printf_P(PSTR("Sonar Sensor\n"));
print_divider(); print_divider();
@ -769,8 +761,6 @@ void report_sonar()
void report_frame() void report_frame()
{ {
//print_blanks(2);
read_EEPROM_frame();
Serial.printf_P(PSTR("Frame\n")); Serial.printf_P(PSTR("Frame\n"));
print_divider(); print_divider();
@ -790,7 +780,6 @@ void report_frame()
void report_radio() void report_radio()
{ {
//print_blanks(2);
Serial.printf_P(PSTR("Radio\n")); Serial.printf_P(PSTR("Radio\n"));
print_divider(); print_divider();
// radio // radio
@ -801,7 +790,6 @@ void report_radio()
void report_gains() void report_gains()
{ {
//print_blanks(2);
Serial.printf_P(PSTR("Gains\n")); Serial.printf_P(PSTR("Gains\n"));
print_divider(); print_divider();
@ -839,7 +827,6 @@ void report_gains()
void report_xtrack() void report_xtrack()
{ {
//print_blanks(2);
Serial.printf_P(PSTR("Crosstrack\n")); Serial.printf_P(PSTR("Crosstrack\n"));
print_divider(); print_divider();
// radio // radio
@ -855,7 +842,6 @@ void report_xtrack()
void report_throttle() void report_throttle()
{ {
//print_blanks(2);
Serial.printf_P(PSTR("Throttle\n")); Serial.printf_P(PSTR("Throttle\n"));
print_divider(); print_divider();
@ -875,7 +861,6 @@ void report_throttle()
void report_imu() void report_imu()
{ {
//print_blanks(2);
Serial.printf_P(PSTR("IMU\n")); Serial.printf_P(PSTR("IMU\n"));
print_divider(); print_divider();
@ -886,14 +871,9 @@ void report_imu()
void report_compass() void report_compass()
{ {
//print_blanks(2);
Serial.printf_P(PSTR("Compass\n")); Serial.printf_P(PSTR("Compass\n"));
print_divider(); print_divider();
read_EEPROM_compass();
//read_EEPROM_compass_declination();
//read_EEPROM_compass_offset();
print_enabled(g.compass_enabled); print_enabled(g.compass_enabled);
// mag declination // mag declination
@ -912,10 +892,8 @@ void report_compass()
void report_flight_modes() void report_flight_modes()
{ {
//print_blanks(2);
Serial.printf_P(PSTR("Flight modes\n")); Serial.printf_P(PSTR("Flight modes\n"));
print_divider(); print_divider();
read_EEPROM_flight_modes();
for(int i = 0; i < 6; i++ ){ for(int i = 0; i < 6; i++ ){
print_switch(i, g.flight_modes[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);
}
*/

2
ArduCopterMega/system.pde
View File

@ -392,7 +392,7 @@ init_compass()
{ {
dcm.set_compass(&compass); dcm.set_compass(&compass);
bool junkbool = compass.init(); 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 Vector3f junkvector = compass.get_offsets(); // load offsets to account for airframe magnetic interference
} }

90
ArduCopterMega/test.pde
View File

@ -351,6 +351,8 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
// IMU // IMU
// --- // ---
read_AHRS(); read_AHRS();
update_trig();
// allow us to zero out sensors with control switches // allow us to zero out sensors with control switches
if(g.rc_5.control_in < 600){ if(g.rc_5.control_in < 600){
@ -367,6 +369,7 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
ts_num++; ts_num++;
if (ts_num == 5){ if (ts_num == 5){
update_alt();
// 10 hz // 10 hz
ts_num = 0; ts_num = 0;
g_gps->longitude = 0; g_gps->longitude = 0;
@ -474,7 +477,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
accels.x, accels.y, accels.z, accels.x, accels.y, accels.z,
gyros.x, gyros.y, gyros.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.roll_sensor,
dcm.pitch_sensor, dcm.pitch_sensor,
dcm.yaw_sensor); dcm.yaw_sensor);
@ -717,10 +720,10 @@ test_wp(uint8_t argc, const Menu::arg *argv)
// save the alitude above home option // save the alitude above home option
if(g.RTL_altitude == -1){ if(g.RTL_altitude < 0){
Serial.printf_P(PSTR("Hold current altitude\n")); Serial.printf_P(PSTR("Hold current altitude\n"));
}else{ }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); 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 static int8_t
test_pressure(uint8_t argc, const Menu::arg *argv) test_pressure(uint8_t argc, const Menu::arg *argv)
{ {
uint32_t sum; Serial.printf_P(PSTR("Uncalibrated relative airpressure\n"));
Serial.printf_P(PSTR("Uncalibrated Abs Airpressure\n"));
Serial.printf_P(PSTR("Altitude is relative to the start of this test\n"));
print_hit_enter(); 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; home.alt = 0;
wp_distance = 0; wp_distance = 0;
init_barometer(); init_barometer();
reset_I();
// to prevent boost from skewing results
cos_pitch_x = cos_roll_x = 1;
while(1){ while(1){
if (millis()-fast_loopTimer > 9) { if (millis() - fast_loopTimer > 100) {
delta_ms_fast_loop = millis() - fast_loopTimer; delta_ms_fast_loop = millis() - fast_loopTimer;
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
fast_loopTimer = millis(); fast_loopTimer = millis();
calc_altitude_error();
calc_nav_throttle();
} }
if (millis()-medium_loopTimer > 100) { if (millis() - medium_loopTimer > 100) {
medium_loopTimer = millis(); medium_loopTimer = millis();
read_radio(); // read the radio first 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); next_WP.alt = max(next_WP.alt, 30);
read_trim_switch(); read_trim_switch();
baro_alt = read_barometer(); update_alt();
current_loc.alt = baro_alt + home.alt; output_auto_throttle();
Serial.printf_P(PSTR("AP: %ld,\tAlt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \t out:%d\n"),
abs_pressure, Serial.printf_P(PSTR("Alt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \tint: %6.2f \tout:%d\n"),
baro_alt, baro_alt,
next_WP.alt, next_WP.alt,
altitude_error, altitude_error,
(int)g.throttle_cruise, (int)g.throttle_cruise,
g.pid_baro_throttle.get_integrator(),
g.rc_3.servo_out); g.rc_3.servo_out);
/* /*
@ -843,29 +833,35 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
test_mag(uint8_t argc, const Menu::arg *argv) test_mag(uint8_t argc, const Menu::arg *argv)
{ {
if(g.compass_enabled) { if(g.compass_enabled) {
print_hit_enter(); //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
while(1){ print_hit_enter();
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);
if(Serial.available() > 0){ while(1){
return (0); 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() void print_hit_enter()