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");
  }
}