void fc_safety_check() {
if (rc_get(RC_THROTTLE) == 0 && rc_get(RC_YAW) > RC_CH4_OUT_MAX/2-10) {
fc_disarm();
}
if (rc_get(RC_THROTTLE) == 0 && rc_get(RC_YAW) < RC_CH4_OUT_MIN/2+10) {
fc_arm();
}
// watchdog to prevent stale imu values
if (imu_rates().x == last_gyro_value) {
gyro_freeze_counter++;
if (gyro_freeze_counter == 500) {
Serial.println("gyro freeze");
fc_emergency_stop();
}
} else {
gyro_freeze_counter = 0;
last_gyro_value = imu_rates().x;
}
if (ANGLE_SAFETY_STOP && (imu_angles().x > 45.0 || imu_angles().x < -45.0
|| imu_angles().y > 45.0 || imu_angles().y < -45.0)) {
Serial.println("angles too high");
fc_emergency_stop();
}
}
void fc_safety_check() {
float yaw = rc_get(RC_YAW);
float yaw_max = rc_out_max(RC_YAW);
if (rc_get(RC_THROTTLE) == 0 && yaw < yaw_max * -0.9) {
fc_disarm();
}
if (rc_get(RC_THROTTLE) == 0 && yaw > yaw_max * 0.9) {
fc_arm();
}
// watchdog to prevent stale imu values
if (imu_rates().x == last_gyro_value) {
gyro_freeze_counter++;
if (gyro_freeze_counter == 500) {
fc_emergency_stop("gyro freeze");
}
} else {
gyro_freeze_counter = 0;
last_gyro_value = imu_rates().x;
}
if (ANGLE_SAFETY_STOP && (imu_angles().x > SAFE_ANGLE || imu_angles().x < -SAFE_ANGLE
|| imu_angles().y > SAFE_ANGLE || imu_angles().y < -SAFE_ANGLE)) {
fc_emergency_stop("angles too high");
}
}