Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-25 18:18:49 -04:00
Code Issues Packages Projects Releases Wiki Activity

Arducopter.pde :

Browse Source 
removed unneeded d_rate_filters
updated Nav routine to handle faster GPS updates
moved calc_XY_velocity to GPS read
added check for duplicate GPS reads
This commit is contained in:
Jason Short 2012-05-17 10:55:13 -07:00
parent 5b3cc413fa
commit be71cbfcc8
1 changed files with 36 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
ArduCopter/ArduCopter.pde
View File

@ -568,9 +568,6 @@ int32_t pitch_axis;
AverageFilterInt32_Size3 roll_rate_d_filter; // filtered acceleration AverageFilterInt32_Size3 roll_rate_d_filter; // filtered acceleration
AverageFilterInt32_Size3 pitch_rate_d_filter; // filtered pitch acceleration AverageFilterInt32_Size3 pitch_rate_d_filter; // filtered pitch acceleration
AverageFilterInt16_Size2 lat_rate_d_filter; // for filtering D term
AverageFilterInt16_Size2 lon_rate_d_filter; // for filtering D term
// Barometer filter // Barometer filter
AverageFilterInt32_Size5 baro_filter; // filtered pitch acceleration AverageFilterInt32_Size5 baro_filter; // filtered pitch acceleration
@ -834,11 +831,8 @@ static uint32_t condition_start;
// IMU variables // IMU variables
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Integration time for the gyros (DCM algorithm) // Integration time for the gyros (DCM algorithm)
// Updated with th efast loop // Updated with the fast loop
static float G_Dt = 0.02; static float G_Dt = 0.02;
// The rotated accelerometer values
// Used by Z accel control, updated at 10hz
Vector3f accels_rot;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Performance monitoring // Performance monitoring
@ -847,7 +841,7 @@ Vector3f accels_rot;
static int16_t perf_mon_counter; static int16_t perf_mon_counter;
// The number of GPS fixes we have had // The number of GPS fixes we have had
static int16_t gps_fix_count; static int16_t gps_fix_count;
// gps_watchdog check for bad reads and if we miss 12 in a row, we stop navigating // gps_watchdog checks for bad reads and if we miss 12 in a row, we stop navigating
// by lowering nav_lat and navlon to 0 gradually // by lowering nav_lat and navlon to 0 gradually
static byte gps_watchdog; static byte gps_watchdog;
@ -919,6 +913,10 @@ void loop()
// Execute the fast loop // Execute the fast loop
// --------------------- // ---------------------
fast_loop(); fast_loop();
} else {
#ifdef DESKTOP_BUILD
usleep(1000);
#endif
} }
// port manipulation for external timing of main loops // port manipulation for external timing of main loops
@ -935,15 +933,9 @@ void loop()
// -------------------------------------------------------------------- // --------------------------------------------------------------------
update_trig(); update_trig();
// update our velocity estimate based on IMU at 50hz
// -------------------------------------------------
//estimate_velocity();
// check for new GPS messages // check for new GPS messages
// -------------------------- // --------------------------
if(!g.retro_loiter && GPS_enabled){ update_GPS();
update_GPS();
}
// perform 10hz tasks // perform 10hz tasks
// ------------------ // ------------------
@ -974,7 +966,7 @@ void loop()
// PORTK |= B01000000; // PORTK |= B01000000;
// PORTK &= B10111111; // PORTK &= B10111111;
// Main loop // Main loop - 100hz
static void fast_loop() static void fast_loop()
{ {
// try to send any deferred messages if the serial port now has // try to send any deferred messages if the serial port now has
@ -986,6 +978,7 @@ static void fast_loop()
read_radio(); read_radio();
// IMU DCM Algorithm // IMU DCM Algorithm
// --------------------
read_AHRS(); read_AHRS();
// custom code/exceptions for flight modes // custom code/exceptions for flight modes
@ -1015,10 +1008,6 @@ static void medium_loop()
case 0: case 0:
medium_loopCounter++; medium_loopCounter++;
if(g.retro_loiter && GPS_enabled){
update_GPS();
}
#if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode #if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode
if(g.compass_enabled){ if(g.compass_enabled){
if (compass.read()) { if (compass.read()) {
@ -1043,38 +1032,21 @@ static void medium_loop()
case 1: case 1:
medium_loopCounter++; medium_loopCounter++;
// Auto control modes: // calculate the copter's desired bearing and WP distance
// ------------------------------------------------------
if(nav_ok){ if(nav_ok){
// clear nav flag // clear nav flag
nav_ok = false; nav_ok = false;
// calculate the copter's desired bearing and WP distance // calculate distance, angles to target
// ------------------------------------------------------ navigate();
if(navigate()){
// this calculates the velocity for Loiter // update flight control system
// only called when there is new data update_navigation();
// ----------------------------------
calc_XY_velocity();
// If we have optFlow enabled we can grab a more accurate speed // update log
// here and override the speed from the GPS if (g.log_bitmask & MASK_LOG_NTUN && motors.armed()){
// ---------------------------------------- Log_Write_Nav_Tuning();
//#ifdef OPTFLOW_ENABLED
//if(g.optflow_enabled && current_loc.alt < 500){
// // optflow wont be enabled on 1280's
// x_GPS_speed = optflow.x_cm;
// y_GPS_speed = optflow.y_cm;
//}
//#endif
// control mode specific updates
// -----------------------------
update_navigation();
if (g.log_bitmask & MASK_LOG_NTUN && motors.armed()){
Log_Write_Nav_Tuning();
}
} }
} }
break; break;
@ -1174,29 +1146,29 @@ static void fifty_hz_loop()
// Read Sonar // Read Sonar
// ---------- // ----------
# if CONFIG_SONAR == ENABLED # if CONFIG_SONAR == ENABLED
if(g.sonar_enabled){ if(g.sonar_enabled){
sonar_alt = sonar.read(); sonar_alt = sonar.read();
} }
#endif #endif
// syncronise optical flow reads with altitude reads // syncronise optical flow reads with altitude reads
#ifdef OPTFLOW_ENABLED #ifdef OPTFLOW_ENABLED
if(g.optflow_enabled){ if(g.optflow_enabled){
update_optical_flow(); update_optical_flow();
} }
#endif #endif
// agmatthews - USERHOOKS
#ifdef USERHOOK_50HZLOOP #ifdef USERHOOK_50HZLOOP
USERHOOK_50HZLOOP USERHOOK_50HZLOOP
#endif #endif
#if HIL_MODE != HIL_MODE_DISABLED && FRAME_CONFIG != HELI_FRAME #if HIL_MODE != HIL_MODE_DISABLED && FRAME_CONFIG != HELI_FRAME
// HIL for a copter needs very fast update of the servo values // HIL for a copter needs very fast update of the servo values
gcs_send_message(MSG_RADIO_OUT); gcs_send_message(MSG_RADIO_OUT);
#endif #endif
camera_stabilization();
# if HIL_MODE == HIL_MODE_DISABLED # if HIL_MODE == HIL_MODE_DISABLED
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST && motors.armed()) if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST && motors.armed())
@ -1206,6 +1178,9 @@ static void fifty_hz_loop()
Log_Write_Raw(); Log_Write_Raw();
#endif #endif
camera_stabilization();
// kick the GCS to process uplink data // kick the GCS to process uplink data
gcs_update(); gcs_update();
gcs_data_stream_send(); gcs_data_stream_send();
@ -1306,7 +1281,7 @@ static void super_slow_loop()
#endif #endif
/* /*
Serial.printf("alt %d, next.alt %d, alt_err: %d, cruise: %d, Alt_I:%1.2f, wp_dist %d, tar_bear %d, home_d %d, homebear %d\n", //Serial.printf("alt %d, next.alt %d, alt_err: %d, cruise: %d, Alt_I:%1.2f, wp_dist %d, tar_bear %d, home_d %d, homebear %d\n",
current_loc.alt, current_loc.alt,
next_WP.alt, next_WP.alt,
altitude_error, altitude_error,
@ -1366,10 +1341,6 @@ static void update_GPS(void)
g_gps->update(); g_gps->update();
update_GPS_light(); update_GPS_light();
//current_loc.lng = 377697000; // Lon * 10 * *7
//current_loc.lat = -1224318000; // Lat * 10 * *7
//current_loc.alt = 100; // alt * 10 * *7
//return;
if(gps_watchdog < 30){ if(gps_watchdog < 30){
gps_watchdog++; gps_watchdog++;
}else{ }else{
@ -1427,6 +1398,8 @@ static 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
calc_XY_velocity();
if (g.log_bitmask & MASK_LOG_GPS && motors.armed()){ if (g.log_bitmask & MASK_LOG_GPS && motors.armed()){
Log_Write_GPS(); Log_Write_GPS();
} }
@ -1604,7 +1577,7 @@ void update_simple_mode(void)
#define THROTTLE_FILTER_SIZE 2 #define THROTTLE_FILTER_SIZE 2
// 50 hz update rate, not 250 // 50 hz update rate
// controls all throttle behavior // controls all throttle behavior
void update_throttle_mode(void) void update_throttle_mode(void)
{ {
@ -1711,7 +1684,7 @@ void update_throttle_mode(void)
} }
// 10hz, don't run up i term // 10hz, don't run up i term
if( motors.auto_armed() == true ){ if(motors.auto_armed() == true){
// how far off are we // how far off are we
altitude_error = get_altitude_error(); altitude_error = get_altitude_error();
@ -2270,6 +2243,7 @@ static void update_nav_wp()
int16_t speed = calc_desired_speed(g.waypoint_speed_max, slow_wp); int16_t speed = calc_desired_speed(g.waypoint_speed_max, slow_wp);
// use error as the desired rate towards the target // use error as the desired rate towards the target
calc_nav_rate(speed); calc_nav_rate(speed);
// rotate pitch and roll to the copter frame of reference // rotate pitch and roll to the copter frame of reference
calc_loiter_pitch_roll(); calc_loiter_pitch_roll();