void pim_router::check_my_address(bool force) {
if (!force && !m_my_address.is_any())
return;
inet6_addr was = m_my_address;
m_my_address = in6addr_any;
const mrd::interface_list &intflist = g_mrd->intflist();
for (mrd::interface_list::const_iterator i = intflist.begin();
i != intflist.end(); ++i) {
if (!i->second->up())
continue;
const std::set<inet6_addr> &globals = i->second->globals();
for (std::set<inet6_addr>::const_iterator j = globals.begin();
j != globals.end(); ++j) {
if (m_my_address.is_any() || *j < m_my_address)
m_my_address = *j;
}
}
if (!(was == m_my_address)) {
if (!m_my_address.is_any()) {
if (should_log(DEBUG))
log().xprintf("Primary global address is"
" %{Addr}.\n", m_my_address);
#ifndef PIM_NO_BSR
if (was.is_any())
bsr().acquired_primary_address();
#endif
} else if (!was.is_any()) {
if (should_log(DEBUG))
log().writeline("Lost primary global address.");
}
}
}
// one_hz_loop - runs at 1Hz
void Copter::one_hz_loop()
{
if (should_log(MASK_LOG_ANY)) {
Log_Write_Data(DATA_AP_STATE, ap.value);
}
update_arming_checks();
if (!motors.armed()) {
// make it possible to change ahrs orientation at runtime during initial config
ahrs.set_orientation();
update_using_interlock();
#if FRAME_CONFIG != HELI_FRAME
// check the user hasn't updated the frame orientation
motors.set_frame_orientation(g.frame_orientation);
// set all throttle channel settings
motors.set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
#endif
}
// update assigned functions and enable auxiliary servos
RC_Channel_aux::enable_aux_servos();
check_usb_mux();
// update position controller alt limits
update_poscon_alt_max();
// enable/disable raw gyro/accel logging
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
// log terrain data
terrain_logging();
adsb.set_is_flying(!ap.land_complete);
}
/*
check for new compass data - 10Hz
*/
void Rover::update_compass(void)
{
if (g.compass_enabled && compass.read()) {
ahrs.set_compass(&compass);
// update offsets
compass.learn_offsets();
if (should_log(MASK_LOG_COMPASS)) {
DataFlash.Log_Write_Compass(compass);
}
} else {
ahrs.set_compass(NULL);
}
}
// read baro and sonar altitude at 10hz
void Copter::update_altitude()
{
// read in baro altitude
read_barometer();
// read in sonar altitude
sonar_alt = read_sonar();
// write altitude info to dataflash logs
if (should_log(MASK_LOG_CTUN)) {
Log_Write_Control_Tuning();
}
}
void pim_interface::handle_join_wc_rpt(group *grp, const inet6_addr &src,
const address_set &pruneaddrs, uint32_t holdtime, bool rpt) {
if (!grp)
return;
pim_group_node *node = (pim_group_node *)grp->node_owned_by(pim);
/// 3.2.2.1.2
if (!node) {
/* Either PIM is disabled for this group or we didn't have
* enough memory in the past */
return;
}
if (node->has_rp() && !(node->rpaddr() == src)) {
/*
* We already have a group instance for G, and the currently
* used RP address differs from the requested one, ignore Join.
*/
return;
}
bool had = node->has_wildcard();
if (!had) {
if (!node->create_wildcard()) {
return;
}
}
node->wildcard()->set_oif(owner(), holdtime);
if (!had) {
rp_source rpsrc;
inet6_addr possiblerp = node->rp_for_group(rpsrc);
if (!(possiblerp == src)) {
if (should_log(DEBUG)) {
log().writeline("RP in J/P message is not the"
"configured one, ignoring Join/Prune.");
return;
}
}
node->set_rp(src, rps_join);
/// 3.2.2.1.5
node->wildcard()->check_upstream_path();
}
handle_join(node, src, holdtime, rpt);
}
/*
set the flight stage
*/
void Plane::set_flight_stage(AP_SpdHgtControl::FlightStage fs)
{
if (fs == flight_stage) {
return;
}
switch (fs) {
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude());
auto_state.land_in_progress = true;
#if GEOFENCE_ENABLED == ENABLED
if (g.fence_autoenable == 1) {
if (! geofence_set_enabled(false, AUTO_TOGGLED)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)");
}
} else if (g.fence_autoenable == 2) {
if (! geofence_set_floor_enabled(false)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
}
}
#endif
break;
case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm", auto_state.takeoff_altitude_rel_cm/100);
auto_state.land_in_progress = false;
break;
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
auto_state.land_in_progress = true;
break;
case AP_SpdHgtControl::FLIGHT_NORMAL:
case AP_SpdHgtControl::FLIGHT_VTOL:
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
auto_state.land_in_progress = false;
break;
}
flight_stage = fs;
if (should_log(MASK_LOG_MODE)) {
Log_Write_Status();
}
}
// ten_hz_logging_loop
// should be run at 10hz
void Copter::ten_hz_logging_loop()
{
// log attitude data if we're not already logging at the higher rate
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
Log_Write_EKF_POS();
}
if (should_log(MASK_LOG_MOTBATT)) {
Log_Write_MotBatt();
}
if (should_log(MASK_LOG_RCIN)) {
DataFlash.Log_Write_RCIN();
if (rssi.enabled()) {
DataFlash.Log_Write_RSSI(rssi);
}
}
if (should_log(MASK_LOG_RCOUT)) {
DataFlash.Log_Write_RCOUT();
}
if (should_log(MASK_LOG_NTUN) && (flightmode->requires_GPS() || landing_with_GPS())) {
pos_control->write_log();
}
if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {
DataFlash.Log_Write_Vibration();
}
if (should_log(MASK_LOG_CTUN)) {
attitude_control->control_monitor_log();
#if PROXIMITY_ENABLED == ENABLED
DataFlash.Log_Write_Proximity(g2.proximity); // Write proximity sensor distances
#endif
#if BEACON_ENABLED == ENABLED
DataFlash.Log_Write_Beacon(g2.beacon);
#endif
}
#if FRAME_CONFIG == HELI_FRAME
Log_Write_Heli();
#endif
}
void Plane::one_second_loop()
{
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
// make it possible to change control channel ordering at runtime
set_control_channels();
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
if (!hal.util->get_soft_armed() && (last_mixer_crc == -1)) {
// if disarmed try to configure the mixer
setup_failsafe_mixing();
}
#endif // CONFIG_HAL_BOARD
// make it possible to change orientation at runtime
ahrs.set_orientation();
adsb.set_stall_speed_cm(airspeed.get_airspeed_min());
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
update_aux();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
#if AP_TERRAIN_AVAILABLE
if (should_log(MASK_LOG_GPS)) {
terrain.log_terrain_data(DataFlash);
}
#endif
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
}
void Plane::one_second_loop()
{
if (should_log(MASK_LOG_CURRENT))
Log_Write_Current();
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
// make it possible to change control channel ordering at runtime
set_control_channels();
// make it possible to change orientation at runtime
ahrs.set_orientation();
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
update_aux();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
crash_detection_update();
#if AP_TERRAIN_AVAILABLE
if (should_log(MASK_LOG_GPS)) {
terrain.log_terrain_data(DataFlash);
}
#endif
// piggyback the status log entry on the MODE log entry flag
if (should_log(MASK_LOG_MODE)) {
Log_Write_Status();
}
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
}
void pim_interface::property_changed(node *n, const char *name) {
if (!strcmp(name, "dr_priority")) {
if (conf()) {
if (should_log(DEBUG))
log().xprintf("Changed DR-Priority to %u\n",
(uint32_t)conf()->dr_priority());
send_hello();
elect_subnet_dr();
}
} else if (!strcmp(name, "hello_interval")) {
update_hello_interval(conf()->hello_interval());
}
}
void pim_interface::found_new_neighbour(pim_neighbour *neigh) {
if (should_log(NORMAL))
log().xprintf("New Neighbour at %{Addr}\n", neigh->localaddr());
send_hello();
#ifndef PIM_NO_BSR
if (am_dr()) {
pim->bsr().found_new_neighbour(neigh);
}
#endif
pim->found_new_neighbour(neigh);
}
/*
read the GPS and update position
*/
void Plane::update_GPS_50Hz(void)
{
static uint32_t last_gps_reading[GPS_MAX_INSTANCES];
gps.update();
for (uint8_t i=0; i<gps.num_sensors(); i++) {
if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
last_gps_reading[i] = gps.last_message_time_ms(i);
if (should_log(MASK_LOG_GPS)) {
Log_Write_GPS(i);
}
}
}
}
/**
handle an updated position from the aircraft
*/
void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg)
{
vehicle.location.lat = msg.lat;
vehicle.location.lng = msg.lon;
vehicle.location.alt = msg.alt/10;
vehicle.relative_alt = msg.relative_alt/10;
vehicle.vel = Vector3f(msg.vx/100.0f, msg.vy/100.0f, msg.vz/100.0f);
vehicle.last_update_us = AP_HAL::micros();
vehicle.last_update_ms = AP_HAL::millis();
// log vehicle as GPS2
if (should_log(MASK_LOG_GPS)) {
Log_Write_Vehicle_Pos(vehicle.location.lat, vehicle.location.lng, vehicle.location.alt, vehicle.vel);
}
}
void StdLogger::logv(const char *p_format, va_list p_list, bool p_err) {
if (!should_log(p_err)) {
return;
}
if (p_err) {
vfprintf(stderr, p_format, p_list);
} else {
vprintf(p_format, p_list);
#ifdef DEBUG_ENABLED
fflush(stdout);
#endif
}
}
void Copter::perf_update(void)
{
if (should_log(MASK_LOG_PM))
Log_Write_Performance();
if (scheduler.debug()) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "PERF: %u/%u %lu %lu\n",
(unsigned)perf_info_get_num_long_running(),
(unsigned)perf_info_get_num_loops(),
(unsigned long)perf_info_get_max_time(),
(unsigned long)perf_info_get_min_time());
}
perf_info_reset();
pmTest1 = 0;
}
void find_data::got_write_token(node_id const& n, std::string write_token)
{
#ifndef TORRENT_DISABLE_LOGGING
auto logger = get_node().observer();
if (logger != nullptr && logger->should_log(dht_logger::traversal))
{
logger->log(dht_logger::traversal
, "[%u] adding write token '%s' under id '%s'"
, id(), aux::to_hex(write_token).c_str()
, aux::to_hex(n).c_str());
}
#endif
m_write_tokens[n] = std::move(write_token);
}
void Rover::update_GPS_50Hz(void)
{
static uint32_t last_gps_reading[GPS_MAX_INSTANCES];
gps.update();
for (uint8_t i=0; i<gps.num_sensors(); i++) {
if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
last_gps_reading[i] = gps.last_message_time_ms(i);
if (should_log(MASK_LOG_GPS)) {
DataFlash.Log_Write_GPS(gps, i, current_loc.alt);
}
}
}
}
// twentyfive_hz_logging - should be run at 25hz
void Copter::twentyfive_hz_logging()
{
#if HIL_MODE != HIL_MODE_DISABLED
// HIL for a copter needs very fast update of the servo values
gcs().send_message(MSG_SERVO_OUTPUT_RAW);
#endif
#if HIL_MODE == HIL_MODE_DISABLED
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_EKF_POS();
}
// log IMU data if we're not already logging at the higher rate
if (should_log(MASK_LOG_IMU) && !should_log(MASK_LOG_IMU_RAW)) {
DataFlash.Log_Write_IMU();
}
#endif
#if PRECISION_LANDING == ENABLED
// log output
Log_Write_Precland();
#endif
}
void mld_interface::handle_mode_change_for_group(int ver, const inet6_addr &reqsrc,
const inet6_addr &grpaddr, int mode, const address_set &srcs) {
const std::set<inet6_addr> &signaling_filter = conf()->signaling_filter();
if (!signaling_filter.empty()) {
bool accept = false;
for (std::set<inet6_addr>::const_iterator i = signaling_filter.begin();
!accept && i != signaling_filter.end(); ++i) {
accept = i->matches(grpaddr);
}
if (!accept) {
if (should_log(DEBUG)) {
log().xprintf("Rejected mode change for group "
"%{Addr} by filter.\n", grpaddr);
}
return;
}
}
if (((mode == MLD_SSM_MODE_INCLUDE || mode == MLD_SSM_CHANGE_TO_INCLUDE) && srcs.empty())
|| (mode == MLD_SSM_ALLOW_SOURCES || mode == MLD_SSM_BLOCK_SOURCES)) {
/* if mode for this record is Include {}, TO_IN {}, ALLOW {A} or BLOCK {A}
* dont create group if it doesnt exist */
group *grp = g_mrd->get_group_by_addr(grpaddr);
if (grp) {
mld_group_interface *oif = mld->match(grp)->local_oif(this);
if (oif)
oif->refresh(reqsrc, mode, srcs);
}
return;
}
create_group_mld_context *ctx = new create_group_mld_context;
if (!ctx)
return;
ctx->iif = owner()->index();
ctx->groupaddr = grpaddr;
ctx->requester = reqsrc;
ctx->mld_mode = mode;
ctx->mld_sources = srcs;
g_mrd->create_group(mld, this, ctx);
}
/*
recalculate the flight_stage
*/
void Plane::update_flight_stage(void)
{
// Update the speed & height controller states
if (auto_throttle_mode && !throttle_suppressed) {
if (control_mode==AUTO) {
if (auto_state.takeoff_complete == false) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF);
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
if ((g.land_abort_throttle_enable && channel_throttle->control_in > 95) ||
auto_state.commanded_go_around ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT){
// abort mode is sticky, it must complete while executing NAV_LAND
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_ABORT);
} else if (auto_state.land_complete == true) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL);
} else if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
float path_progress = location_path_proportion(current_loc, prev_WP_loc, next_WP_loc);
bool lined_up = abs(nav_controller->bearing_error_cd()) < 1000;
bool below_prev_WP = current_loc.alt < prev_WP_loc.alt;
if ((path_progress > 0.15f && lined_up && below_prev_WP) || path_progress > 0.5f) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH);
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
}
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
target_airspeed_cm,
flight_stage,
auto_state.takeoff_pitch_cd,
throttle_nudge,
tecs_hgt_afe(),
aerodynamic_load_factor);
if (should_log(MASK_LOG_TECS)) {
Log_Write_TECS_Tuning();
}
}
// tell AHRS the airspeed to true airspeed ratio
airspeed.set_EAS2TAS(barometer.get_EAS2TAS());
}
// set_home - sets ahrs home (used for RTL) to specified location
// initialises inertial nav and compass on first call
// returns true if home location set successfully
bool Sub::set_home(const Location& loc, bool lock)
{
// check location is valid
if (loc.lat == 0 && loc.lng == 0) {
return false;
}
// check if EKF origin has been set
Location ekf_origin;
if (!ahrs.get_origin(ekf_origin)) {
return false;
}
const bool home_was_set = ahrs.home_is_set();
// set ahrs home (used for RTL)
ahrs.set_home(loc);
// init inav and compass declination
if (!home_was_set) {
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
scaleLongDown = longitude_scale(loc);
// record home is set
Log_Write_Event(DATA_SET_HOME);
// log new home position which mission library will pull from ahrs
if (should_log(MASK_LOG_CMD)) {
AP_Mission::Mission_Command temp_cmd;
if (mission.read_cmd_from_storage(0, temp_cmd)) {
DataFlash.Log_Write_Mission_Cmd(mission, temp_cmd);
}
}
}
// lock home position
if (lock) {
ahrs.lock_home();
}
// log ahrs home and ekf origin dataflash
ahrs.Log_Write_Home_And_Origin();
// send new home and ekf origin to GCS
gcs().send_home();
gcs().send_ekf_origin();
// return success
return true;
}
// set_home - sets ahrs home (used for RTL) to specified location
// initialises inertial nav and compass on first call
// returns true if home location set successfully
bool Copter::set_home(const Location& loc, bool lock)
{
// check location is valid
if (loc.lat == 0 && loc.lng == 0) {
return false;
}
// check EKF origin has been set
Location ekf_origin;
if (!ahrs.get_origin(ekf_origin)) {
return false;
}
// check home is close to EKF origin
if (far_from_EKF_origin(loc)) {
return false;
}
const bool home_was_set = ahrs.home_is_set();
// set ahrs home (used for RTL)
ahrs.set_home(loc);
// init inav and compass declination
if (!home_was_set) {
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
scaleLongDown = longitude_scale(loc);
// record home is set
Log_Write_Event(DATA_SET_HOME);
#if MODE_AUTO_ENABLED == ENABLED
// log new home position which mission library will pull from ahrs
if (should_log(MASK_LOG_CMD)) {
AP_Mission::Mission_Command temp_cmd;
if (mode_auto.mission.read_cmd_from_storage(0, temp_cmd)) {
logger.Write_Mission_Cmd(mode_auto.mission, temp_cmd);
}
}
#endif
}
// lock home position
if (lock) {
ahrs.lock_home();
}
// return success
return true;
}
// read_battery - reads battery voltage and current and invokes failsafe
// should be called at 10hz
void Plane::read_battery(void)
{
battery.read();
compass.set_current(battery.current_amps());
if (!usb_connected &&
hal.util->get_soft_armed() &&
battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah)) {
low_battery_event();
}
if (should_log(MASK_LOG_CURRENT)) {
Log_Write_Current();
}
}
/**
handle an updated position from the aircraft
*/
void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg)
{
vehicle.location.lat = msg.lat;
vehicle.location.lng = msg.lon;
vehicle.location.alt = msg.alt/10;
vehicle.heading = msg.hdg * 0.01f;
vehicle.ground_speed = norm(msg.vx, msg.vy) * 0.01f;
vehicle.last_update_us = AP_HAL::micros();
vehicle.last_update_ms = AP_HAL::millis();
// log vehicle as GPS2
if (should_log(MASK_LOG_GPS)) {
DataFlash.Log_Write_GPS(gps, 1);
}
}
// update_batt_compass - read battery and compass
// should be called at 10hz
void Copter::update_batt_compass(void)
{
// read battery before compass because it may be used for motor interference compensation
read_battery();
if(g.compass_enabled) {
// update compass with throttle value - used for compassmot
compass.set_throttle(motors.get_throttle()/1000.0f);
compass.read();
// log compass information
if (should_log(MASK_LOG_COMPASS)) {
DataFlash.Log_Write_Compass(compass);
}
}
}
void AP_InertialSensor::BatchSampler::sample(uint8_t _instance, AP_InertialSensor::IMU_SENSOR_TYPE _type, uint64_t sample_us, const Vector3f &_sample)
{
if (!should_log(_instance, _type)) {
return;
}
if (data_write_offset == 0) {
measurement_started_us = sample_us;
}
data_x[data_write_offset] = multiplier*_sample.x;
data_y[data_write_offset] = multiplier*_sample.y;
data_z[data_write_offset] = multiplier*_sample.z;
data_write_offset++; // may unblock the reading process
}
void mld_router::icmp_message_available(interface *intf, const in6_addr &src,
const in6_addr &dst, icmp6_hdr *hdr,
int len) {
if (IN6_IS_ADDR_MULTICAST(&dst)) {
mld_interface *mintf = get_interface(intf->index());
if (mintf) {
mintf->icmp_message_available(src, dst, hdr, len);
} else if (_is_mld_message(hdr->icmp6_type)
&& should_log(MESSAGE_ERR)) {
mld->log_router_desc(intf->log()).writeline(
"Incoming MLD message discarded, "
"interface disabled.");
}
}
}
// update_batt_compass - read battery and compass
// should be called at 10hz
void Sub::update_batt_compass()
{
// read battery before compass because it may be used for motor interference compensation
battery.read();
if (g.compass_enabled) {
// update compass with throttle value - used for compassmot
compass.set_throttle(motors.get_throttle());
compass.read();
// log compass information
if (should_log(MASK_LOG_COMPASS) && !ahrs.have_ekf_logging()) {
DataFlash.Log_Write_Compass();
}
}
}
// update AHRS system
void Rover::ahrs_update()
{
update_soft_armed();
#if HIL_MODE != HIL_MODE_DISABLED
// update hil before AHRS update
gcs_update();
#endif
// AHRS may use movement to calculate heading
update_ahrs_flyforward();
ahrs.update();
// update position
have_position = ahrs.get_position(current_loc);
// update home from EKF if necessary
update_home_from_EKF();
// if using the EKF get a speed update now (from accelerometers)
Vector3f velocity;
if (ahrs.get_velocity_NED(velocity)) {
ground_speed = norm(velocity.x, velocity.y);
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
ground_speed = ahrs.groundspeed();
}
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
}
if (should_log(MASK_LOG_IMU)) {
DataFlash.Log_Write_IMU();
}
}
// update AHRS system
void Plane::ahrs_update()
{
hal.util->set_soft_armed(arming.is_armed() &&
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
#if HIL_SUPPORT
if (g.hil_mode == 1) {
// update hil before AHRS update
gcs_update();
}
#endif
ahrs.update();
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
}
if (should_log(MASK_LOG_IMU)) {
Log_Write_IMU();
DataFlash.Log_Write_IMUDT(ins);
}
// calculate a scaled roll limit based on current pitch
roll_limit_cd = g.roll_limit_cd * cosf(ahrs.pitch);
pitch_limit_min_cd = aparm.pitch_limit_min_cd * fabsf(cosf(ahrs.roll));
// updated the summed gyro used for ground steering and
// auto-takeoff. Dot product of DCM.c with gyro vector gives earth
// frame yaw rate
steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt;
steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err);
// update inertial_nav for quadplane
quadplane.inertial_nav.update(G_Dt);
}
