Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Converted Millis to Micros

Browse Source 
added navigation check to return a failed navigation
This commit is contained in:
Jason Short 2011-09-19 22:24:57 -07:00
parent 0b39920871
commit b2b28cc90c
4 changed files with 26 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
ArduCopter/ArduCopter.pde
View File

@ -488,14 +488,11 @@ static byte gps_watchdog;
// System Timers
// --------------
static unsigned long fast_loopTimer; // Time in miliseconds of main control loop
static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds
static byte medium_loopCounter; // Counters for branching from main control loop to slower loops
static uint16_t throttle_timer;
static float delta_throttle;
static unsigned long fiftyhz_loopTimer;
static uint8_t delta_ms_fiftyhz;
static byte slow_loopCounter;
static int superslow_loopCounter;
@ -504,7 +501,6 @@ static byte simple_timer; // for limiting the execution of flight mode thi
static float dTnav; // Delta Time in milliseconds for navigation computations
static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav
//static float load; // % MCU cycles used
static byte counter_one_herz;
static bool GPS_enabled = false;
@ -522,10 +518,14 @@ void setup() {
void loop()
{
long timer = micros();
// We want this to execute fast
// ----------------------------
if (millis() - fast_loopTimer >= 4) {
if ((timer - fast_loopTimer) >= 4000) {
//PORTK |= B00010000;
G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops
fast_loopTimer = timer;
//Serial.printf("%1.5f\n", G_Dt);
// Execute the fast loop
// ---------------------
@ -533,9 +533,8 @@ void loop()
}
//PORTK &= B11101111;
if (millis() - fiftyhz_loopTimer > 19) {
delta_ms_fiftyhz = millis() - fiftyhz_loopTimer;
fiftyhz_loopTimer = millis();
if ((timer - fiftyhz_loopTimer) >= 20000) {
fiftyhz_loopTimer = timer;
//PORTK |= B01000000;
// reads all of the necessary trig functions for cameras, throttle, etc.
@ -584,14 +583,10 @@ static void fast_loop()
// IMU DCM Algorithm
read_AHRS();
fast_loopTimer = millis();
delta_ms_fast_loop = millis() - fast_loopTimer;
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by PI Loops
// Look for slow loop times
// ------------------------
if (delta_ms_fast_loop > G_Dt_max)
G_Dt_max = delta_ms_fast_loop;
//if (delta_ms_fast_loop > G_Dt_max)
// G_Dt_max = delta_ms_fast_loop;
// custom code/exceptions for flight modes
// ---------------------------------------
@ -669,14 +664,15 @@ static void medium_loop()
// calculate the copter's desired bearing and WP distance
// ------------------------------------------------------
navigate();
if(navigate()){
// control mode specific updates
// -----------------------------
update_navigation();
// control mode specific updates
// -----------------------------
update_navigation();
if (g.log_bitmask & MASK_LOG_NTUN)
Log_Write_Nav_Tuning();
if (g.log_bitmask & MASK_LOG_NTUN)
Log_Write_Nav_Tuning();
}
}
break;

5
ArduCopter/Attitude.pde
View File

@ -100,8 +100,9 @@ get_nav_throttle(long z_error, int target_speed)
rate_error = target_speed - altitude_rate;
rate_error = constrain(rate_error, -110, 110);
delta_throttle = (float)(millis() - throttle_timer)/1000.0;
throttle_timer = millis();
long timer = micros();
delta_throttle = (float)(timer - throttle_timer)/1000000.0;
throttle_timer = timer;
return g.pi_throttle.get_pi(rate_error, delta_throttle);
}

14
ArduCopter/navigation.pde
View File

@ -3,17 +3,10 @@
//****************************************************************
// Function that will calculate the desired direction to fly and distance
//****************************************************************
static void navigate()
static byte navigate()
{
// do not navigate with corrupt data
// ---------------------------------
if (g_gps->fix == 0){
g_gps->new_data = false;
return;
}
if(next_WP.lat == 0){
return;
return 0;
}
// waypoint distance from plane
@ -24,12 +17,13 @@ static void navigate()
//gcs.send_text_P(SEVERITY_HIGH,PSTR("<navigate> WP error - distance < 0"));
//Serial.println(wp_distance,DEC);
//print_current_waypoints();
return;
return 0;
}
// target_bearing is where we should be heading
// --------------------------------------------
target_bearing = get_bearing(&current_loc, &next_WP);
return 1;
}
static bool check_missed_wp()

6
ArduCopter/test.pde
View File

@ -394,9 +394,9 @@ test_imu(uint8_t argc, const Menu::arg *argv)
while(1){
//delay(20);
if (millis() - fast_loopTimer >= 5) {
delta_ms_fast_loop = millis() - fast_loopTimer;
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
fast_loopTimer = millis();
//delta_ms_fast_loop = millis() - fast_loopTimer;
//G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
//fast_loopTimer = millis();
// IMU
// ---