C++ (Cpp) FLOAT_RATES_OF_EULER_DOTの例

コード例 #1

ファイル: ahrs_on_synth.c プロジェクト: EwoudSmeur/paparazzi

static void traj_sineX_quad_update(void)
{

  const float om = RadOfDeg(10);

  if (aos.time > (M_PI / om)) {
    const float a = 20;

    struct FloatVect3 jerk;
    VECT3_ASSIGN(jerk           , -a * om * om * om * cos(om * aos.time), 0, 0);
    VECT3_ASSIGN(aos.ltp_accel  , -a * om * om   * sin(om * aos.time), 0, 0);
    VECT3_ASSIGN(aos.ltp_vel    ,  a * om      * cos(om * aos.time), 0, 0);
    VECT3_ASSIGN(aos.ltp_pos    ,  a         * sin(om * aos.time), 0, 0);

    // this is based on differential flatness of the quad
    EULERS_ASSIGN(aos.ltp_to_imu_euler,    0., atan2(aos.ltp_accel.x, 9.81), 0.);
    float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);
    const struct FloatEulers e_dot = {
      0.,
      9.81 * jerk.x / ((9.81 * 9.81) + (aos.ltp_accel.x * aos.ltp_accel.x)),
      0.
    };
    FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);
  }
}

コード例 #2

ファイル: ahrs_on_synth.c プロジェクト: AshuLara/paparazzi

static void traj_coordinated_circle_update(void) {
  const float speed = 15;  // m/s
  const float R = 100;     // radius in m
  float omega = speed / R;
  // tan phi = v^2/Rg
  float phi = atan2(speed*speed, R*9.81);
  if ( aos.time > 2.*M_PI/omega) {
    VECT3_ASSIGN(aos.ltp_pos,                R*cos(omega*aos.time),              R*sin(omega*aos.time), 0.);
    VECT3_ASSIGN(aos.ltp_vel,         -omega*R*sin(omega*aos.time),        omega*R*cos(omega*aos.time), 0.);
    VECT3_ASSIGN(aos.ltp_accel, -omega*omega*R*cos(omega*aos.time), -omega*omega*R*sin(omega*aos.time), 0.);
    
  
    //  float psi = atan2(aos.ltp_pos.y, aos.ltp_pos.x);
    float psi = M_PI_2 + omega*aos.time; while (psi>M_PI) psi -= 2.*M_PI;
    EULERS_ASSIGN(aos.ltp_to_imu_euler,   phi, 0, psi);
    FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
    
    struct FloatEulers e_dot;
    EULERS_ASSIGN(e_dot, 0., 0., omega);
    FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);
  }

}

コード例 #3

ファイル: ahrs_on_synth.c プロジェクト: AshuLara/paparazzi

static void  traj_stop_stop_x_update(void){

  const float t0 = 5.;
  const float dt_jerk = 0.75;
  const float dt_nojerk = 10.;
  const float val_jerk = 5.;

  FLOAT_VECT3_INTEGRATE_FI(aos.ltp_pos, aos.ltp_vel, aos.dt);
  FLOAT_VECT3_INTEGRATE_FI(aos.ltp_vel, aos.ltp_accel, aos.dt);
  FLOAT_VECT3_INTEGRATE_FI(aos.ltp_accel, aos.ltp_jerk, aos.dt);

  if (aos.time < t0) return;
  else if (aos.time < t0+dt_jerk) {
    VECT3_ASSIGN(aos.ltp_jerk           ,  val_jerk, 0., 0.); }
  else if (aos.time < t0 + 2.*dt_jerk) {
    VECT3_ASSIGN(aos.ltp_jerk           , -val_jerk, 0., 0.); }
  else if (aos.time < t0 + 2.*dt_jerk + dt_nojerk) {
    VECT3_ASSIGN(aos.ltp_jerk           ,       0. , 0., 0.); }
  else if (aos.time < t0 + 3.*dt_jerk + dt_nojerk) {
    VECT3_ASSIGN(aos.ltp_jerk           , -val_jerk, 0., 0.); }
  else if (aos.time < t0 + 4.*dt_jerk + dt_nojerk) {
    VECT3_ASSIGN(aos.ltp_jerk           ,  val_jerk, 0., 0.); }
  else {
    VECT3_ASSIGN(aos.ltp_jerk           ,       0. , 0., 0.); }
  
    
  // this is based on differential flatness of the quad
  EULERS_ASSIGN(aos.ltp_to_imu_euler,    0., atan2(aos.ltp_accel.x, 9.81), 0.);
  FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
  const struct FloatEulers e_dot = {
    0., 
    9.81*aos.ltp_jerk.x / ( (9.81*9.81) + (aos.ltp_accel.x*aos.ltp_accel.x)),
    0. };
  FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);

}