Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APM: allow for navigation by dead-reckoning 

we now ask AHRS if we have a position estimate, and use that if
available
This commit is contained in:
Andrew Tridgell 2012-08-11 12:01:08 +10:00
parent 708280511c
commit 9d9c7b0455
3 changed files with 13 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
ArduPlane/ArduPlane.pde
View File

@ -390,6 +390,9 @@ static int16_t ground_start_avg;
// If we do not detect GPS at startup, we stop trying and assume GPS is not connected // If we do not detect GPS at startup, we stop trying and assume GPS is not connected
static bool GPS_enabled = false; static bool GPS_enabled = false;
// true if we have a position estimate from AHRS
static bool have_position;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Location & Navigation // Location & Navigation
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -854,12 +857,9 @@ Serial.println(tempaccel.z, DEC);
// ------------------------------- // -------------------------------
read_control_switch(); read_control_switch();
if(g_gps->new_data){ // calculate the plane's desired bearing
g_gps->new_data = false; // -------------------------------------
// calculate the plane's desired bearing navigate();
// -------------------------------------
navigate();
}
break; break;
@ -985,7 +985,12 @@ static void update_GPS(void)
g_gps->update(); g_gps->update();
update_GPS_light(); update_GPS_light();
// get position from AHRS
have_position = ahrs.get_position(&current_loc);
if (g_gps->new_data && g_gps->fix) { if (g_gps->new_data && g_gps->fix) {
g_gps->new_data = false;
// for performance // for performance
// --------------- // ---------------
gps_fix_count++; gps_fix_count++;
@ -1021,10 +1026,6 @@ static void update_GPS(void)
} }
} }
current_loc.lng = g_gps->longitude; // Lon * 10**7
current_loc.lat = g_gps->latitude; // Lat * 10**7
// see if we've breached the geo-fence // see if we've breached the geo-fence
geofence_check(false); geofence_check(false);
} }

2
ArduPlane/GCS_Mavlink.pde
View File

@ -242,7 +242,7 @@ static void NOINLINE send_location(mavlink_channel_t chan)
g_gps->ground_speed * rot.a.x, // X speed cm/s g_gps->ground_speed * rot.a.x, // X speed cm/s
g_gps->ground_speed * rot.b.x, // Y speed cm/s g_gps->ground_speed * rot.b.x, // Y speed cm/s
g_gps->ground_speed * rot.c.x, g_gps->ground_speed * rot.c.x,
g_gps->ground_course); // course in 1/100 degree ahrs.yaw_sensor);
} }
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)

4
ArduPlane/navigation.pde
View File

@ -7,9 +7,7 @@ static void navigate()
{ {
// do not navigate with corrupt data // do not navigate with corrupt data
// --------------------------------- // ---------------------------------
if (g_gps->fix == 0) if (!have_position) {
{
g_gps->new_data = false;
return; return;
} }