C++ (Cpp) INT32_COURSE_NORMALIZE Beispiele

Beispiel #1

Datei: state.c Projekt: CheBuzz/paparazzi

void stateCalcHorizontalSpeedDir_i(void) {
  if (bit_is_set(state.speed_status, SPEED_HDIR_I))
    return;

  if (bit_is_set(state.speed_status, SPEED_HDIR_F)){
    state.h_speed_dir_i = SPEED_BFP_OF_REAL(state.h_speed_dir_f);
  }
  else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
    INT32_ATAN2(state.h_speed_dir_i, state.ned_speed_i.y, state.ned_speed_i.x);
    INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
  }
  else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
    INT32_ATAN2(state.h_speed_dir_i, state.enu_speed_i.x, state.enu_speed_i.y);
    INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
  }
  else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
    SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
    SetBit(state.speed_status, SPEED_NED_I);
    INT32_ATAN2(state.h_speed_dir_i, state.ned_speed_i.y, state.ned_speed_i.x);
    INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
  }
  else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
    SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
    SetBit(state.speed_status, SPEED_ENU_I);
    INT32_ATAN2(state.h_speed_dir_i, state.enu_speed_i.x, state.enu_speed_i.y);
    INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
  }
  /* set bit to indicate this representation is computed */
  SetBit(state.speed_status, SPEED_HDIR_I);
}

Beispiel #2

Datei: rotorcraft_cam.c Projekt: AULA-PPZ/paparazzi

void rotorcraft_cam_periodic(void)
{

  switch (rotorcraft_cam_mode) {
    case ROTORCRAFT_CAM_MODE_NONE:
#if ROTORCRAFT_CAM_USE_TILT
      rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL;
#endif
#if ROTORCRAFT_CAM_USE_PAN
      rotorcraft_cam_pan = stateGetNedToBodyEulers_i()->psi;
#endif
      break;
    case ROTORCRAFT_CAM_MODE_MANUAL:
      // nothing to do here, just apply tilt pwm at the end
      break;
    case ROTORCRAFT_CAM_MODE_HEADING:
#if ROTORCRAFT_CAM_USE_TILT_ANGLES
      Bound(rotorcraft_cam_tilt, CT_MIN, CT_MAX);
      rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) /
                                (CAM_TA_MAX - CAM_TA_MIN);
#endif
#if ROTORCRAFT_CAM_USE_PAN
      INT32_COURSE_NORMALIZE(rotorcraft_cam_pan);
      nav_heading = rotorcraft_cam_pan;
#endif
      break;
    case ROTORCRAFT_CAM_MODE_WP:
#ifdef ROTORCRAFT_CAM_TRACK_WP
      {
        struct Int32Vect2 diff;
        VECT2_DIFF(diff, waypoints[ROTORCRAFT_CAM_TRACK_WP], *stateGetPositionEnu_i());
        INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC);
        rotorcraft_cam_pan = int32_atan2(diff.x, diff.y);
        nav_heading = rotorcraft_cam_pan;
#if ROTORCRAFT_CAM_USE_TILT_ANGLES
        int32_t dist, height;
        dist = INT32_VECT2_NORM(diff);
        height = (waypoints[ROTORCRAFT_CAM_TRACK_WP].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC;
        rotorcraft_cam_tilt = int32_atan2(height, dist);
        Bound(rotorcraft_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
        rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) /
                                  (CAM_TA_MAX - CAM_TA_MIN);
#endif
      }
#endif
      break;
    default:
      break;
  }
#if ROTORCRAFT_CAM_USE_TILT
  ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm);
#endif
}