void Rover::send_extended_status1(mavlink_channel_t chan)
{
int16_t battery_current = -1;
int8_t battery_remaining = -1;
if (battery.has_current() && battery.healthy()) {
battery_remaining = battery.capacity_remaining_pct();
battery_current = battery.current_amps() * 100;
}
update_sensor_status_flags();
mavlink_msg_sys_status_send(
chan,
control_sensors_present,
control_sensors_enabled,
control_sensors_health,
static_cast<uint16_t>(scheduler.load_average() * 1000),
battery.voltage() * 1000, // mV
battery_current, // in 10mA units
battery_remaining, // in %
0, // comm drops %,
0, // comm drops in pkts,
0, 0, 0, 0);
}
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 HAVE_PX4_MIXER
if (!hal.util->get_soft_armed() && (last_mixer_crc == -1)) {
// if disarmed try to configure the mixer
setup_failsafe_mixing();
}
#endif // CONFIG_HAL_BOARD
#if HAL_WITH_IO_MCU
iomcu.setup_mixing(&rcmap, g.override_channel.get(), g.mixing_gain, g2.manual_rc_mask);
#endif
// make it possible to change orientation at runtime
ahrs.set_orientation();
adsb.set_stall_speed_cm(aparm.airspeed_min);
adsb.set_max_speed(aparm.airspeed_max);
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
SRV_Channels::enable_aux_servos();
// 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
// update home position if armed and gps position has
// changed. Update every 5s at most
if (!arming.is_armed() &&
gps.last_message_time_ms() - last_home_update_ms > 5000 &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
last_home_update_ms = gps.last_message_time_ms();
update_home();
// reset the landing altitude correction
landing.alt_offset = 0;
}
// update error mask of sensors and subsystems. The mask uses the
// MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
// indicates that the sensor or subsystem is present but not
// functioning correctly
update_sensor_status_flags();
}
/*
once a second events
*/
void Rover::one_second_loop(void)
{
if (should_log(MASK_LOG_CURRENT)) {
Log_Write_Current();
}
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
// allow orientation change at runtime to aid config
ahrs.set_orientation();
set_control_channels();
// cope with changes to aux functions
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;
// cope with changes to mavlink system ID
mavlink_system.sysid = g.sysid_this_mav;
static uint8_t counter;
counter++;
// write perf data every 20s
if (counter % 10 == 0) {
if (scheduler.debug() != 0) {
hal.console->printf("G_Dt_max=%u\n", G_Dt_max);
}
if (should_log(MASK_LOG_PM)) {
Log_Write_Performance();
}
G_Dt_max = 0;
resetPerfData();
}
// save compass offsets once a minute
if (counter >= 60) {
if (g.compass_enabled) {
compass.save_offsets();
}
counter = 0;
}
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
// update error mask of sensors and subsystems. The mask uses the
// MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
// indicates that the sensor or subsystem is present but not
// functioning correctly
update_sensor_status_flags();
}
/*
once a second events
*/
void Rover::one_second_loop(void)
{
// send a heartbeat
gcs().send_message(MSG_HEARTBEAT);
// allow orientation change at runtime to aid config
ahrs.set_orientation();
set_control_channels();
// cope with changes to aux functions
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;
// cope with changes to mavlink system ID
mavlink_system.sysid = g.sysid_this_mav;
static uint8_t counter;
counter++;
// save compass offsets once a minute
if (counter >= 60) {
if (g.compass_enabled) {
compass.save_offsets();
}
counter = 0;
}
// update home position if not soft armed and gps position has
// changed. Update every 1s at most
if (!hal.util->get_soft_armed() &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
update_home();
}
// update error mask of sensors and subsystems. The mask uses the
// MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
// indicates that the sensor or subsystem is present but not
// functioning correctly
update_sensor_status_flags();
}
// 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);
}
arming.update();
if (!motors->armed()) {
// make it possible to change ahrs orientation at runtime during initial config
ahrs.set_orientation();
update_using_interlock();
// check the user hasn't updated the frame class or type
motors->set_frame_class_and_type((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
#if FRAME_CONFIG != HELI_FRAME
// 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
SRV_Channels::enable_aux_servos();
// log terrain data
terrain_logging();
#if ADSB_ENABLED == ENABLED
adsb.set_is_flying(!ap.land_complete);
#endif
// update error mask of sensors and subsystems. The mask uses the
// MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
// indicates that the sensor or subsystem is present but not
// functioning correctly
update_sensor_status_flags();
// init compass location for declination
init_compass_location();
}
