mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
Arducopter.pde :
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
parent
5b3cc413fa
commit
be71cbfcc8
@ -568,9 +568,6 @@ int32_t pitch_axis;
|
||||
AverageFilterInt32_Size3 roll_rate_d_filter; // filtered 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
|
||||
AverageFilterInt32_Size5 baro_filter; // filtered pitch acceleration
|
||||
|
||||
@ -834,11 +831,8 @@ static uint32_t condition_start;
|
||||
// IMU variables
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Integration time for the gyros (DCM algorithm)
|
||||
// Updated with th efast loop
|
||||
// Updated with the fast loop
|
||||
static float G_Dt = 0.02;
|
||||
// The rotated accelerometer values
|
||||
// Used by Z accel control, updated at 10hz
|
||||
Vector3f accels_rot;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Performance monitoring
|
||||
@ -847,7 +841,7 @@ Vector3f accels_rot;
|
||||
static int16_t perf_mon_counter;
|
||||
// The number of GPS fixes we have had
|
||||
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
|
||||
static byte gps_watchdog;
|
||||
|
||||
@ -919,6 +913,10 @@ void loop()
|
||||
// Execute the fast loop
|
||||
// ---------------------
|
||||
fast_loop();
|
||||
} else {
|
||||
#ifdef DESKTOP_BUILD
|
||||
usleep(1000);
|
||||
#endif
|
||||
}
|
||||
|
||||
// port manipulation for external timing of main loops
|
||||
@ -935,15 +933,9 @@ void loop()
|
||||
// --------------------------------------------------------------------
|
||||
update_trig();
|
||||
|
||||
// update our velocity estimate based on IMU at 50hz
|
||||
// -------------------------------------------------
|
||||
//estimate_velocity();
|
||||
|
||||
// check for new GPS messages
|
||||
// --------------------------
|
||||
if(!g.retro_loiter && GPS_enabled){
|
||||
update_GPS();
|
||||
}
|
||||
update_GPS();
|
||||
|
||||
// perform 10hz tasks
|
||||
// ------------------
|
||||
@ -974,7 +966,7 @@ void loop()
|
||||
// PORTK |= B01000000;
|
||||
// PORTK &= B10111111;
|
||||
|
||||
// Main loop
|
||||
// Main loop - 100hz
|
||||
static void fast_loop()
|
||||
{
|
||||
// try to send any deferred messages if the serial port now has
|
||||
@ -986,6 +978,7 @@ static void fast_loop()
|
||||
read_radio();
|
||||
|
||||
// IMU DCM Algorithm
|
||||
// --------------------
|
||||
read_AHRS();
|
||||
|
||||
// custom code/exceptions for flight modes
|
||||
@ -1015,10 +1008,6 @@ static void medium_loop()
|
||||
case 0:
|
||||
medium_loopCounter++;
|
||||
|
||||
if(g.retro_loiter && GPS_enabled){
|
||||
update_GPS();
|
||||
}
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode
|
||||
if(g.compass_enabled){
|
||||
if (compass.read()) {
|
||||
@ -1043,38 +1032,21 @@ static void medium_loop()
|
||||
case 1:
|
||||
medium_loopCounter++;
|
||||
|
||||
// Auto control modes:
|
||||
// calculate the copter's desired bearing and WP distance
|
||||
// ------------------------------------------------------
|
||||
if(nav_ok){
|
||||
// clear nav flag
|
||||
nav_ok = false;
|
||||
|
||||
// calculate the copter's desired bearing and WP distance
|
||||
// ------------------------------------------------------
|
||||
if(navigate()){
|
||||
// calculate distance, angles to target
|
||||
navigate();
|
||||
|
||||
// this calculates the velocity for Loiter
|
||||
// only called when there is new data
|
||||
// ----------------------------------
|
||||
calc_XY_velocity();
|
||||
// update flight control system
|
||||
update_navigation();
|
||||
|
||||
// If we have optFlow enabled we can grab a more accurate speed
|
||||
// here and override the speed from the GPS
|
||||
// ----------------------------------------
|
||||
//#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();
|
||||
}
|
||||
// update log
|
||||
if (g.log_bitmask & MASK_LOG_NTUN && motors.armed()){
|
||||
Log_Write_Nav_Tuning();
|
||||
}
|
||||
}
|
||||
break;
|
||||
@ -1174,29 +1146,29 @@ static void fifty_hz_loop()
|
||||
// Read Sonar
|
||||
// ----------
|
||||
# if CONFIG_SONAR == ENABLED
|
||||
if(g.sonar_enabled){
|
||||
sonar_alt = sonar.read();
|
||||
}
|
||||
if(g.sonar_enabled){
|
||||
sonar_alt = sonar.read();
|
||||
}
|
||||
#endif
|
||||
|
||||
// syncronise optical flow reads with altitude reads
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
if(g.optflow_enabled){
|
||||
update_optical_flow();
|
||||
}
|
||||
if(g.optflow_enabled){
|
||||
update_optical_flow();
|
||||
}
|
||||
#endif
|
||||
|
||||
// agmatthews - USERHOOKS
|
||||
|
||||
#ifdef USERHOOK_50HZLOOP
|
||||
USERHOOK_50HZLOOP
|
||||
USERHOOK_50HZLOOP
|
||||
#endif
|
||||
|
||||
|
||||
#if HIL_MODE != HIL_MODE_DISABLED && FRAME_CONFIG != HELI_FRAME
|
||||
// HIL for a copter needs very fast update of the servo values
|
||||
gcs_send_message(MSG_RADIO_OUT);
|
||||
#endif
|
||||
|
||||
camera_stabilization();
|
||||
|
||||
# if HIL_MODE == HIL_MODE_DISABLED
|
||||
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST && motors.armed())
|
||||
@ -1206,6 +1178,9 @@ static void fifty_hz_loop()
|
||||
Log_Write_Raw();
|
||||
#endif
|
||||
|
||||
|
||||
camera_stabilization();
|
||||
|
||||
// kick the GCS to process uplink data
|
||||
gcs_update();
|
||||
gcs_data_stream_send();
|
||||
@ -1306,7 +1281,7 @@ static void super_slow_loop()
|
||||
#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,
|
||||
next_WP.alt,
|
||||
altitude_error,
|
||||
@ -1366,10 +1341,6 @@ static void update_GPS(void)
|
||||
g_gps->update();
|
||||
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){
|
||||
gps_watchdog++;
|
||||
}else{
|
||||
@ -1427,6 +1398,8 @@ static void update_GPS(void)
|
||||
current_loc.lng = g_gps->longitude; // Lon * 10 * *7
|
||||
current_loc.lat = g_gps->latitude; // Lat * 10 * *7
|
||||
|
||||
calc_XY_velocity();
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_GPS && motors.armed()){
|
||||
Log_Write_GPS();
|
||||
}
|
||||
@ -1604,7 +1577,7 @@ void update_simple_mode(void)
|
||||
|
||||
#define THROTTLE_FILTER_SIZE 2
|
||||
|
||||
// 50 hz update rate, not 250
|
||||
// 50 hz update rate
|
||||
// controls all throttle behavior
|
||||
void update_throttle_mode(void)
|
||||
{
|
||||
@ -1711,7 +1684,7 @@ void update_throttle_mode(void)
|
||||
}
|
||||
|
||||
// 10hz, don't run up i term
|
||||
if( motors.auto_armed() == true ){
|
||||
if(motors.auto_armed() == true){
|
||||
|
||||
// how far off are we
|
||||
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);
|
||||
// use error as the desired rate towards the target
|
||||
calc_nav_rate(speed);
|
||||
|
||||
// rotate pitch and roll to the copter frame of reference
|
||||
calc_loiter_pitch_roll();
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user