// read_battery - check battery voltage and current and invoke failsafe if necessary // called at 10hz void Sub::read_battery(void) { battery.read(); // update compass with current value if (battery.has_current()) { compass.set_current(battery.current_amps()); } // update motors with voltage and current if (battery.get_type() != AP_BattMonitor::BattMonitor_TYPE_NONE) { motors.set_voltage(battery.voltage()); } if (battery.has_current()) { motors.set_current(battery.current_amps()); } failsafe_battery_check(); // log battery info to the dataflash if (should_log(MASK_LOG_CURRENT)) { Log_Write_Current(); } }
// read_battery - check battery voltage and current and invoke failsafe if necessary // called at 10hz void Copter::read_battery(void) { battery.read(); // update compass with current value if (battery.has_current()) { compass.set_current(battery.current_amps()); } // update motors with voltage and current if (battery.get_type() != AP_BattMonitor::BattMonitor_TYPE_NONE) { motors.set_voltage(battery.voltage()); } if (battery.has_current()) { motors.set_current(battery.current_amps()); } // check for low voltage or current if the low voltage check hasn't already been triggered // we only check when we're not powered by USB to avoid false alarms during bench tests if (!ap.usb_connected && !failsafe.battery && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah)) { failsafe_battery_event(); } // log battery info to the dataflash if (should_log(MASK_LOG_CURRENT)) { Log_Write_Current(); } }
/* 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(); }
// 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(); } }
/* 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=%lu\n", (unsigned long)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)); }
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(); // determine if we are flying or not determine_is_flying(); // 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 // 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)); }