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

Copter: add support for GPS fix type 2D

This commit is contained in:
Randy Mackay 2013-03-25 16:25:13 +09:00 committed by rmackay9
parent 24044dc0c4
commit f7d977fe37
3 changed files with 49 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
ArduCopter/ArduCopter.pde
View File

@ -1374,52 +1374,48 @@ static void update_GPS(void)
g_gps->update();
update_GPS_light();
set_gps_healthy(g_gps->status() == g_gps->GPS_OK);
set_gps_healthy(g_gps->status() >= GPS::GPS_OK_FIX_3D);
if (g_gps->new_data && g_gps->fix) {
if (g_gps->new_data && last_gps_time != g_gps->time && g_gps->status() >= GPS::GPS_OK_FIX_2D) {
// clear new data flag
g_gps->new_data = false;
// check for duiplicate GPS messages
if(last_gps_time != g_gps->time) {
// save GPS time so we don't get duplicate reads
last_gps_time = g_gps->time;
// for performance monitoring
// --------------------------
gps_fix_count++;
// log location if we have at least a 2D fix
if (g.log_bitmask & MASK_LOG_GPS && motors.armed()) {
Log_Write_GPS();
}
if(ground_start_count > 1) {
ground_start_count--;
} else if (ground_start_count == 1) {
// We countdown N number of good GPS fixes
// so that the altitude is more accurate
// -------------------------------------
if (g_gps->latitude == 0) {
ground_start_count = 5;
// for performance monitoring
gps_fix_count++;
// check if we can initialise home yet
if (!ap.home_is_set) {
// if we have a 3d lock and valid location
if(g_gps->status() >= GPS::GPS_OK_FIX_3D && g_gps->latitude != 0) {
if( ground_start_count > 0 ) {
ground_start_count--;
}else{
// after 10 successful reads store home location
// ap.home_is_set will be true so this will only happen once
ground_start_count = 0;
init_home();
if (g.compass_enabled) {
// Set compass declination automatically
compass.set_initial_location(g_gps->latitude, g_gps->longitude);
}
// save home to eeprom (we must have a good fix to have reached this point)
init_home();
ground_start_count = 0;
}
}
if (g.log_bitmask & MASK_LOG_GPS && motors.armed()) {
Log_Write_GPS();
}else{
// start again if we lose 3d lock
ground_start_count = 10;
}
#if HIL_MODE == HIL_MODE_ATTITUDE // only execute in HIL mode
ap_system.alt_sensor_flag = true;
#endif
}
// save GPS time so we don't get duplicate reads
last_gps_time = g_gps->time;
}
// check for loss of gps

11
ArduCopter/GCS_Mavlink.pde
View File

@ -132,7 +132,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
@ -225,7 +225,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();
@ -289,15 +289,10 @@ static void NOINLINE send_hwstatus(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

45
ArduCopter/leds.pde
View File

@ -19,30 +19,31 @@ 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):
if(ap.home_is_set) { // JLN update
digitalWriteFast(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix AND home is set.
} else {
digitalWriteFast(C_LED_PIN, LED_OFF);
}
break;
case (1):
if (g_gps->valid_read == true) {
ap_system.GPS_light = !ap_system.GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
if (ap_system.GPS_light) {
digitalWriteFast(C_LED_PIN, LED_OFF);
}else{
digitalWriteFast(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;
ap_system.GPS_light = !ap_system.GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
if (ap_system.GPS_light) {
digitalWriteFast(C_LED_PIN, LED_OFF);
}else{
digitalWriteFast(C_LED_PIN, LED_ON);
}
}
g_gps->valid_read = false;
}
break;
break;
default:
digitalWriteFast(C_LED_PIN, LED_OFF);
break;
case GPS::GPS_OK_FIX_3D:
if(ap.home_is_set) {
digitalWriteFast(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix AND home is set.
} else {
digitalWriteFast(C_LED_PIN, LED_OFF);
}
break;
default:
digitalWriteFast(C_LED_PIN, LED_OFF);
break;
}
}