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());
}