/*
read update GPS position - 10Hz update
*/
void Plane::update_GPS_10Hz(void)
{
static uint32_t last_gps_msg_ms;
if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
last_gps_msg_ms = gps.last_message_time_ms();
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 (current_loc.lat == 0) {
ground_start_count = 5;
} else {
init_home();
// set system clock for log timestamps
uint64_t gps_timestamp = gps.time_epoch_usec();
hal.util->set_system_clock(gps_timestamp);
// update signing timestamp
GCS_MAVLINK::update_signing_timestamp(gps_timestamp);
if (g.compass_enabled) {
// Set compass declination automatically
const Location &loc = gps.location();
compass.set_initial_location(loc.lat, loc.lng);
}
ground_start_count = 0;
}
}
// see if we've breached the geo-fence
geofence_check(false);
#if CAMERA == ENABLED
if (camera.update_location(current_loc, plane.ahrs ) == true) {
do_take_picture();
}
#endif
if (!hal.util->get_soft_armed()) {
update_home();
// reset the landing altitude correction
auto_state.land_alt_offset = 0;
}
// update wind estimate
ahrs.estimate_wind();
} else if (gps.status() < AP_GPS::GPS_OK_FIX_3D && ground_start_count != 0) {
// lost 3D fix, start again
ground_start_count = 5;
}
calc_gndspeed_undershoot();
}
/*
read update GPS position - 10Hz update
*/
void Plane::update_GPS_10Hz(void)
{
// get position from AHRS
have_position = ahrs.get_position(current_loc);
static uint32_t last_gps_msg_ms;
if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
last_gps_msg_ms = gps.last_message_time_ms();
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 (current_loc.lat == 0) {
ground_start_count = 5;
} else {
init_home();
// set system clock for log timestamps
hal.util->set_system_clock(gps.time_epoch_usec());
if (g.compass_enabled) {
// Set compass declination automatically
const Location &loc = gps.location();
compass.set_initial_location(loc.lat, loc.lng);
}
ground_start_count = 0;
}
}
// see if we've breached the geo-fence
geofence_check(false);
#if CAMERA == ENABLED
if (camera.update_location(current_loc) == true) {
do_take_picture();
}
#endif
if (!hal.util->get_soft_armed()) {
update_home();
}
// update wind estimate
ahrs.estimate_wind();
}
calc_gndspeed_undershoot();
}
/*
read update GPS position - 10Hz update
*/
void Plane::update_GPS_10Hz(void)
{
static uint32_t last_gps_msg_ms;
if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
last_gps_msg_ms = gps.last_message_time_ms();
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 (current_loc.lat == 0 && current_loc.lng == 0) {
ground_start_count = 5;
} else {
if (!set_home_persistently(gps.location())) {
// silently ignore failure...
}
next_WP_loc = prev_WP_loc = home;
if (AP::compass().enabled()) {
// Set compass declination automatically
const Location &loc = gps.location();
compass.set_initial_location(loc.lat, loc.lng);
}
ground_start_count = 0;
}
}
// see if we've breached the geo-fence
geofence_check(false);
#if CAMERA == ENABLED
camera.update();
#endif
// update wind estimate
ahrs.estimate_wind();
} else if (gps.status() < AP_GPS::GPS_OK_FIX_3D && ground_start_count != 0) {
// lost 3D fix, start again
ground_start_count = 5;
}
calc_gndspeed_undershoot();
}
