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

cleanup

Browse Source 
added more ifdef for optflow
This commit is contained in:
Jason Short 2011-09-15 23:41:15 -07:00
parent 9a508a20f4
commit 0d31d775b8
3 changed files with 15 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/ArduCopter.pde
View File

@ -204,7 +204,7 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
#endif
#endif
#elif HIL_MODE == HIL_MODE_ATTITUDE
#if HIL_MODE == HIL_MODE_ATTITUDE
#ifdef OPTFLOW_ENABLED
AP_OpticalFlow_ADNS3080 optflow(&dcm);
#endif
@ -617,6 +617,7 @@ static void medium_loop()
medium_loopCounter++;
#ifdef OPTFLOW_ENABLED
if(g.optflow_enabled){
optflow.read();
optflow.update_position(cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow
@ -626,6 +627,7 @@ static void medium_loop()
Log_Write_Optflow();
}
}
#endif
if(GPS_enabled){
update_GPS();

2
ArduCopter/config.h
View File

@ -210,7 +210,7 @@
#define OPTFLOW_ENABLED
#endif
#define OPTFLOW_ENABLED
//#define OPTFLOW_ENABLED
#ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI)
# define OPTFLOW DISABLED

24
ArduCopter/navigation.pde
View File

@ -97,14 +97,19 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed
// find the rates:
float temp = radians((float)g_gps->ground_course/100.0);
// calc the cos of the error to tell how fast we are moving towards the target in cm
if(g.optflow_enabled && current_loc.alt < 500 && g_gps->ground_speed < 150){
x_actual_speed = optflow.vlon * 10;
y_actual_speed = optflow.vlat * 10;
}else{
#ifdef OPTFLOW_ENABLED
// calc the cos of the error to tell how fast we are moving towards the target in cm
if(g.optflow_enabled && current_loc.alt < 500 && g_gps->ground_speed < 150){
x_actual_speed = optflow.vlon * 10;
y_actual_speed = optflow.vlat * 10;
}else{
x_actual_speed = (float)g_gps->ground_speed * sin(temp);
y_actual_speed = (float)g_gps->ground_speed * cos(temp);
}
#else
x_actual_speed = (float)g_gps->ground_speed * sin(temp);
y_actual_speed = (float)g_gps->ground_speed * cos(temp);
}
#endif
y_rate_error = y_target_speed - y_actual_speed; // 413
y_rate_error = constrain(y_rate_error, -600, 600); // added a rate error limit to keep pitching down to a minimum
@ -128,13 +133,6 @@ static void calc_nav_pitch_roll()
nav_pitch = -nav_pitch;
}
// ------------------------------
/*static void calc_bearing_error()
{
bearing_error = nav_bearing - dcm.yaw_sensor;
bearing_error = wrap_180(bearing_error);
}*/
static long get_altitude_error()
{
return next_WP.alt - current_loc.alt;