// 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();
DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
if (should_log(MASK_LOG_PID)) {
DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDY_MSG, g.pid_rate_yaw.get_pid_info() );
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() );
}
}
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) && (mode_requires_GPS(control_mode) || landing_with_GPS())) {
Log_Write_Nav_Tuning();
}
if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {
DataFlash.Log_Write_Vibration(ins);
}
#if FRAME_CONFIG == HELI_FRAME
Log_Write_Heli();
#endif
}
void Sub::failsafe_sensors_check(void)
{
if (!ap.depth_sensor_present) {
return;
}
// We need a depth sensor to do any sort of auto z control
if (sensor_health.depth) {
failsafe.sensor_health = false;
return;
}
// only report once
if (failsafe.sensor_health) {
return;
}
failsafe.sensor_health = true;
gcs().send_text(MAV_SEVERITY_CRITICAL, "Depth sensor error!");
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_SENSORS, ERROR_CODE_BAD_DEPTH);
if (control_mode == ALT_HOLD || control_mode == SURFACE || mode_requires_GPS(control_mode)) {
// This should always succeed
if (!set_mode(MANUAL, MODE_REASON_BAD_DEPTH)) {
// We should never get here
init_disarm_motors();
}
}
}
// ten_hz_logging_loop
// should be run at 10hz
void Sub::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();
DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
if (should_log(MASK_LOG_PID)) {
DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info());
DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info());
DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info());
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info());
}
}
if (should_log(MASK_LOG_MOTBATT)) {
Log_Write_MotBatt();
}
if (should_log(MASK_LOG_RCIN)) {
DataFlash.Log_Write_RCIN();
}
if (should_log(MASK_LOG_RCOUT)) {
DataFlash.Log_Write_RCOUT();
}
if (should_log(MASK_LOG_NTUN) && mode_requires_GPS(control_mode)) {
Log_Write_Nav_Tuning();
}
if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {
DataFlash.Log_Write_Vibration(ins);
}
if (should_log(MASK_LOG_CTUN)) {
attitude_control.control_monitor_log();
}
}
// failsafe_ekf_event - perform ekf failsafe
void Copter::failsafe_ekf_event()
{
// return immediately if ekf failsafe already triggered
if (failsafe.ekf) {
return;
}
// do nothing if motors disarmed
if (!motors.armed()) {
return;
}
// do nothing if not in GPS flight mode and ekf-action is not land-even-stabilize
if (!mode_requires_GPS(control_mode) && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) {
return;
}
// EKF failsafe event has occurred
failsafe.ekf = true;
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_OCCURRED);
// take action based on fs_ekf_action parameter
switch (g.fs_ekf_action) {
case FS_EKF_ACTION_ALTHOLD:
// AltHold
if (failsafe.radio || !set_mode(ALT_HOLD, MODE_REASON_EKF_FAILSAFE)) {
set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE);
}
break;
default:
set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE);
break;
}
// if flight mode is already LAND ensure it's not the GPS controlled LAND
if (control_mode == LAND) {
land_do_not_use_GPS();
}
}
// performs pre_arm gps related checks and returns true if passed
bool Copter::pre_arm_gps_checks(bool display_failure)
{
// always check if inertial nav has started and is ready
if (!ahrs.healthy()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Nav Checks");
}
return false;
}
// check if flight mode requires GPS
bool gps_required = mode_requires_GPS(control_mode);
#if AC_FENCE == ENABLED
// if circular fence is enabled we need GPS
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) {
gps_required = true;
}
#endif
// return true if GPS is not required
if (!gps_required) {
AP_Notify::flags.pre_arm_gps_check = true;
return true;
}
// ensure GPS is ok
if (!position_ok()) {
if (display_failure) {
const char *reason = ahrs.prearm_failure_reason();
if (reason) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: %s", reason);
} else {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Need 3D Fix");
}
}
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
// check EKF compass variance is below failsafe threshold
float vel_variance, pos_variance, hgt_variance, tas_variance;
Vector3f mag_variance;
Vector2f offset;
ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset);
if (mag_variance.length() >= g.fs_ekf_thresh) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF compass variance");
}
return false;
}
// check home and EKF origin are not too far
if (far_from_EKF_origin(ahrs.get_home())) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF-home variance");
}
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
// return true immediately if gps check is disabled
if (!(g.arming_check == ARMING_CHECK_ALL || g.arming_check & ARMING_CHECK_GPS)) {
AP_Notify::flags.pre_arm_gps_check = true;
return true;
}
// warn about hdop separately - to prevent user confusion with no gps lock
if (gps.get_hdop() > g.gps_hdop_good) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: High GPS HDOP");
}
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
// if we got here all must be ok
AP_Notify::flags.pre_arm_gps_check = true;
return true;
}
// performs pre_arm gps related checks and returns true if passed
bool Copter::pre_arm_gps_checks(bool display_failure)
{
// always check if inertial nav has started and is ready
if(!ahrs.get_NavEKF().healthy()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Waiting for Nav Checks"));
}
return false;
}
// return true immediately if gps check is disabled
if (!(g.arming_check == ARMING_CHECK_ALL || g.arming_check & ARMING_CHECK_GPS)) {
AP_Notify::flags.pre_arm_gps_check = true;
return true;
}
// check if flight mode requires GPS
bool gps_required = mode_requires_GPS(control_mode);
#if AC_FENCE == ENABLED
// if circular fence is enabled we need GPS
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) {
gps_required = true;
}
#endif
// return true if GPS is not required
if (!gps_required) {
AP_Notify::flags.pre_arm_gps_check = true;
return true;
}
// ensure GPS is ok
if (!position_ok()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Need 3D Fix"));
}
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
// check home and EKF origin are not too far
if (far_from_EKF_origin(ahrs.get_home())) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: EKF-home variance"));
}
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
// warn about hdop separately - to prevent user confusion with no gps lock
if (gps.get_hdop() > g.gps_hdop_good) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: High GPS HDOP"));
}
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
// if we got here all must be ok
AP_Notify::flags.pre_arm_gps_check = true;
return true;
}
