aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: add support for GPS fix type 2D

Browse Source
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

4
ArduPlane/Attitude.pde
View File

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

11
ArduPlane/GCS_Mavlink.pde
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<<2); // compass present
} }
control_sensors_present |= (1<<3); // absolute pressure sensor 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<<5); // GPS present
} }
control_sensors_present |= (1<<10); // 3D angular rate control control_sensors_present |= (1<<10); // 3D angular rate control
@ -250,7 +250,7 @@ static void NOINLINE send_location(mavlink_channel_t chan)
// positions. // positions.
// If we don't have a GPS fix then we are dead reckoning, and will // If we don't have a GPS fix then we are dead reckoning, and will
// use the current boot time as the fix time. // 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; fix_time = g_gps->last_fix_time;
} else { } else {
fix_time = millis(); 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) 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( mavlink_msg_gps_raw_int_send(
chan, chan,
g_gps->last_fix_time*(uint64_t)1000, g_gps->last_fix_time*(uint64_t)1000,
fix, g_gps->status(),
g_gps->latitude, // in 1E7 degrees g_gps->latitude, // in 1E7 degrees
g_gps->longitude, // in 1E7 degrees g_gps->longitude, // in 1E7 degrees
g_gps->altitude * 10, // in mm g_gps->altitude * 10, // in mm

2
ArduPlane/commands_logic.pde
View File

@ -583,7 +583,7 @@ static void do_change_speed()
static void do_set_home() 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(); init_home();
} else { } else {
home.id = MAV_CMD_NAV_WAYPOINT; home.id = MAV_CMD_NAV_WAYPOINT;

2
ArduPlane/navigation.pde
View File

@ -100,7 +100,7 @@ static void calc_gndspeed_undershoot()
{ {
// Function is overkill, but here in case we want to add filtering // Function is overkill, but here in case we want to add filtering
// later // 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; groundspeed_undershoot = (g.min_gndspeed_cm > 0) ? (g.min_gndspeed_cm - g_gps->ground_speed) : 0;
} }
} }

36
ArduPlane/system.pde
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 // GPS LED on if we have a fix or Blink GPS LED if we are receiving data
// --------------------------------------------------------------------- // ---------------------------------------------------------------------
switch (g_gps->status()) { switch (g_gps->status()) {
case (2): case GPS::NO_FIX:
digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix. case GPS::GPS_OK_FIX_2D:
break; // check if we've blinked since the last gps update
if (g_gps->valid_read) {
case (1): g_gps->valid_read = false;
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
GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock if (GPS_light) {
if (GPS_light) { digitalWrite(C_LED_PIN, LED_OFF);
digitalWrite(C_LED_PIN, LED_OFF); }else{
} else { digitalWrite(C_LED_PIN, LED_ON);
digitalWrite(C_LED_PIN, LED_ON); }
} }
g_gps->valid_read = false; break;
}
break;
default: case GPS::GPS_OK_FIX_3D:
digitalWrite(C_LED_PIN, LED_OFF); digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix AND home is set.
break; break;
default:
digitalWrite(C_LED_PIN, LED_OFF);
break;
} }
} }