Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

Added optimizations for radian calls to remove a division.

Browse Source 
added a protection for entering AP modes without Home being set by GPS lock.
This commit is contained in:
Jason Short 2011-11-19 11:08:03 -08:00
parent 791fd194a4
commit 35524f6da7
3 changed files with 20 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/defines.h
View File

@ -332,6 +332,8 @@ enum gcs_severity {
#define B_LED_PIN 36 #define B_LED_PIN 36
#define C_LED_PIN 35 #define C_LED_PIN 35
// RADIANS
#define RADX100 0.000174533
// EEPROM addresses // EEPROM addresses
#define EEPROM_MAX_ADDR 4096 #define EEPROM_MAX_ADDR 4096

15
ArduCopter/navigation.pde
View File

@ -67,7 +67,8 @@ static void calc_loiter(int x_error, int y_error)
int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav); int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav);
// find the rates: // find the rates:
float temp = radians((float)g_gps->ground_course/100.0); //float temp = radians((float)g_gps->ground_course/100.0);
float temp = g_gps->ground_course * RADX100;
#ifdef OPTFLOW_ENABLED #ifdef OPTFLOW_ENABLED
// calc the cos of the error to tell how fast we are moving towards the target in cm // calc the cos of the error to tell how fast we are moving towards the target in cm
@ -172,7 +173,8 @@ static void calc_nav_rate(int max_speed)
} }
// XXX target_angle should be the original desired target angle! // XXX target_angle should be the original desired target angle!
float temp = radians((target_bearing - g_gps->ground_course)/100.0); //float temp = radians((target_bearing - g_gps->ground_course)/100.0);
float temp = (target_bearing - g_gps->ground_course) * RADX100;
// push us towards the original track // push us towards the original track
update_crosstrack(); update_crosstrack();
@ -209,7 +211,10 @@ static void update_crosstrack(void)
// Crosstrack Error // Crosstrack Error
// ---------------- // ----------------
if (cross_track_test() < 5000) { // If we are too far off or too close we don't do track following if (cross_track_test() < 5000) { // If we are too far off or too close we don't do track following
crosstrack_error = sin(radians((target_bearing - original_target_bearing) / 100)) * wp_distance; // Meters we are off track line float temp = (target_bearing - original_target_bearing) * RADX100;
//radians((target_bearing - original_target_bearing) / 100)
crosstrack_error = sin(temp) * wp_distance; // Meters we are off track line
crosstrack_error = constrain(crosstrack_error * 4, -1200, 1200); crosstrack_error = constrain(crosstrack_error * 4, -1200, 1200);
} }
} }
@ -225,7 +230,9 @@ static int32_t cross_track_test()
// nav_roll, nav_pitch // nav_roll, nav_pitch
static void calc_nav_pitch_roll() static void calc_nav_pitch_roll()
{ {
float temp = radians((float)(9000 - (dcm.yaw_sensor - original_target_bearing))/100.0); float temp = (9000l - (dcm.yaw_sensor - original_target_bearing)) * RADX100;
//t: 1.5465, t1: -10.9451, t2: 1.5359, t3: 1.5465
float _cos_yaw_x = cos(temp); float _cos_yaw_x = cos(temp);
float _sin_yaw_y = sin(temp); float _sin_yaw_y = sin(temp);

7
ArduCopter/system.pde
View File

@ -359,6 +359,13 @@ static void set_mode(byte mode)
return; return;
} }
// if we don't have GPS lock
if(home_is_set == false){
// our max mode should be
if (mode > ALT_HOLD)
mode = STABILIZE;
}
old_control_mode = control_mode; old_control_mode = control_mode;
control_mode = mode; control_mode = mode;