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