C++ (Cpp) imu_rates Examples

Example #1

File: flight_controller.cpp Project: marvelousmishig/rmb_multicopter

void compute_pids() {
  pid(PID_RATE_X)->input = imu_rates().x;
  pid(PID_RATE_Y)->input = imu_rates().y;
  pid(PID_ANGLE_X)->input = imu_angles().x;
  pid(PID_ANGLE_Y)->input = imu_angles().y;
  pid(PID_RATE_Z)->input = imu_rates().z;

  if (flight_mode == STABILIZE) {
    pid(PID_ANGLE_X)->setpoint = rc_get(RC_ROLL);
    pid(PID_ANGLE_Y)->setpoint = rc_get(RC_PITCH);

    pid_compute(PID_ANGLE_X);
    pid_compute(PID_ANGLE_Y);

    pid(PID_RATE_X)->setpoint = pid(PID_ANGLE_X)->output;
    pid(PID_RATE_Y)->setpoint = pid(PID_ANGLE_Y)->output;
  } else {
    pid(PID_RATE_X)->setpoint = rc_get(RC_ROLL);
    pid(PID_RATE_Y)->setpoint = rc_get(RC_PITCH);
  }

  pid(PID_RATE_Z)->setpoint = rc_get(RC_YAW);

  pid_compute(PID_RATE_X);
  pid_compute(PID_RATE_Y);
  pid_compute(PID_RATE_Z);
}

Example #2

File: flight_controller.cpp Project: marvelousmishig/rmb_multicopter

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

Example #3

File: flight_controller.c Project: sashabaranov/rmb_multicopter

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