/*
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();
}
void Plane::update_alt()
{
barometer.update();
if (should_log(MASK_LOG_IMU)) {
Log_Write_Baro();
}
// calculate the sink rate.
float sink_rate;
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
sink_rate = vel.z;
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
sink_rate = gps.velocity().z;
} else {
sink_rate = -barometer.get_climb_rate();
}
// low pass the sink rate to take some of the noise out
auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
geofence_check(true);
update_flight_stage();
}
void Plane::update_alt()
{
barometer.update();
if (should_log(MASK_LOG_IMU)) {
Log_Write_Baro();
}
// calculate the sink rate.
float sink_rate;
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
sink_rate = vel.z;
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
sink_rate = gps.velocity().z;
} else {
sink_rate = -barometer.get_climb_rate();
}
// low pass the sink rate to take some of the noise out
auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
geofence_check(true);
update_flight_stage();
if (auto_throttle_mode && !throttle_suppressed) {
bool is_doing_auto_land = false;
float distance_beyond_land_wp = 0;
switch (flight_stage) {
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
is_doing_auto_land = true;
if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
distance_beyond_land_wp = get_distance(current_loc, next_WP_loc);
}
break;
default:
break;
}
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
target_airspeed_cm,
flight_stage,
is_doing_auto_land,
distance_beyond_land_wp,
get_takeoff_pitch_min_cd(),
throttle_nudge,
tecs_hgt_afe(),
aerodynamic_load_factor);
if (should_log(MASK_LOG_TECS)) {
Log_Write_TECS_Tuning();
}
}
}
/*
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();
}
void Plane::update_alt()
{
barometer.update();
// calculate the sink rate.
float sink_rate;
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
sink_rate = vel.z;
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
sink_rate = gps.velocity().z;
} else {
sink_rate = -barometer.get_climb_rate();
}
// low pass the sink rate to take some of the noise out
auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
geofence_check(true);
update_flight_stage();
if (auto_throttle_mode && !throttle_suppressed) {
float distance_beyond_land_wp = 0;
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
distance_beyond_land_wp = get_distance(current_loc, next_WP_loc);
}
bool soaring_active = false;
#if SOARING_ENABLED == ENABLED
if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) {
soaring_active = true;
}
#endif
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
target_airspeed_cm,
flight_stage,
distance_beyond_land_wp,
get_takeoff_pitch_min_cd(),
throttle_nudge,
tecs_hgt_afe(),
aerodynamic_load_factor,
soaring_active);
}
}
/*
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();
}
void Plane::update_alt()
{
barometer.update();
if (should_log(MASK_LOG_IMU)) {
Log_Write_Baro();
}
// calculate the sink rate.
float sink_rate;
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
sink_rate = vel.z;
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
sink_rate = gps.velocity().z;
} else {
sink_rate = -barometer.get_climb_rate();
}
// low pass the sink rate to take some of the noise out
auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
geofence_check(true);
update_flight_stage();
if (auto_throttle_mode && !throttle_suppressed) {
float distance_beyond_land_wp = 0;
if (auto_state.land_in_progress && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
distance_beyond_land_wp = get_distance(current_loc, next_WP_loc);
}
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
target_airspeed_cm,
flight_stage,
auto_state.land_in_progress,
distance_beyond_land_wp,
get_takeoff_pitch_min_cd(),
throttle_nudge,
tecs_hgt_afe(),
aerodynamic_load_factor);
}
}