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