Plane: add support for GPS fix type 2D

This commit is contained in:
Randy Mackay 2013-03-25 16:25:24 +09:00 committed by rmackay9
parent f7d977fe37
commit c2055557f5
5 changed files with 26 additions and 29 deletions

View File

@ -384,7 +384,7 @@ static void throttle_slew_limit(int16_t last_throttle)
*/
static bool auto_takeoff_check(void)
{
if (g_gps == NULL || g_gps->status() != GPS::GPS_OK) {
if (g_gps == NULL || g_gps->status() != GPS::GPS_OK_FIX_3D) {
// no auto takeoff without GPS lock
return false;
}
@ -448,7 +448,7 @@ static bool suppress_throttle(void)
}
if (g_gps != NULL &&
g_gps->status() == GPS::GPS_OK &&
g_gps->status() >= GPS::GPS_OK_FIX_2D &&
g_gps->ground_speed >= 500) {
// we're moving at more than 5 m/s
throttle_suppressed = false;

View File

@ -134,7 +134,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
control_sensors_present |= (1<<2); // compass present
}
control_sensors_present |= (1<<3); // absolute pressure sensor present
if (g_gps != NULL && g_gps->status() == GPS::GPS_OK) {
if (g_gps != NULL && g_gps->status() >= GPS::NO_FIX) {
control_sensors_present |= (1<<5); // GPS present
}
control_sensors_present |= (1<<10); // 3D angular rate control
@ -250,7 +250,7 @@ static void NOINLINE send_location(mavlink_channel_t chan)
// positions.
// If we don't have a GPS fix then we are dead reckoning, and will
// use the current boot time as the fix time.
if (g_gps->status() == GPS::GPS_OK) {
if (g_gps->status() >= GPS::GPS_OK_FIX_2D) {
fix_time = g_gps->last_fix_time;
} else {
fix_time = millis();
@ -285,15 +285,10 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
{
uint8_t fix = g_gps->status();
if (fix == GPS::GPS_OK) {
fix = 3;
}
mavlink_msg_gps_raw_int_send(
chan,
g_gps->last_fix_time*(uint64_t)1000,
fix,
g_gps->status(),
g_gps->latitude, // in 1E7 degrees
g_gps->longitude, // in 1E7 degrees
g_gps->altitude * 10, // in mm

View File

@ -583,7 +583,7 @@ static void do_change_speed()
static void do_set_home()
{
if (next_nonnav_command.p1 == 1 && g_gps->status() == GPS::GPS_OK) {
if (next_nonnav_command.p1 == 1 && g_gps->status() == GPS::GPS_OK_FIX_3D) {
init_home();
} else {
home.id = MAV_CMD_NAV_WAYPOINT;

View File

@ -100,7 +100,7 @@ static void calc_gndspeed_undershoot()
{
// Function is overkill, but here in case we want to add filtering
// later
if (g_gps && g_gps->status() == GPS::GPS_OK) {
if (g_gps && g_gps->status() >= GPS::GPS_OK_FIX_2D) {
groundspeed_undershoot = (g.min_gndspeed_cm > 0) ? (g.min_gndspeed_cm - g_gps->ground_speed) : 0;
}
}

View File

@ -490,25 +490,27 @@ static void update_GPS_light(void)
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
// ---------------------------------------------------------------------
switch (g_gps->status()) {
case (2):
digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix.
break;
case (1):
if (g_gps->valid_read == true) {
GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
if (GPS_light) {
digitalWrite(C_LED_PIN, LED_OFF);
} else {
digitalWrite(C_LED_PIN, LED_ON);
case GPS::NO_FIX:
case GPS::GPS_OK_FIX_2D:
// check if we've blinked since the last gps update
if (g_gps->valid_read) {
g_gps->valid_read = false;
GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
if (GPS_light) {
digitalWrite(C_LED_PIN, LED_OFF);
}else{
digitalWrite(C_LED_PIN, LED_ON);
}
}
g_gps->valid_read = false;
}
break;
break;
default:
digitalWrite(C_LED_PIN, LED_OFF);
break;
case GPS::GPS_OK_FIX_3D:
digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix AND home is set.
break;
default:
digitalWrite(C_LED_PIN, LED_OFF);
break;
}
}