C++ DegOfRad函数代码示例

static void test_doubles(void) {

  printf("\n--- enu_of_ecef double ---\n");
  //  struct LlaCoor_f ref_coor;
  //  ref_coor.lat = RAD_OF_DEG(43.605278);
  //  ref_coor.lon = RAD_OF_DEG(1.442778);
  //  ref_coor.alt = 180.0;

  struct EcefCoor_d ref_coor = { 4624497.0 , 116475.0, 4376563.0};
  printf("ecef0 : (%.02f,%.02f,%.02f)\n", ref_coor.x, ref_coor.y, ref_coor.z);

  struct LtpDef_d ltp_def;
  ltp_def_from_ecef_d(&ltp_def, &ref_coor);

  printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), ltp_def.lla.alt);

  struct EcefCoor_d my_ecef_point = ref_coor;
  struct EnuCoor_d  my_enu_point;
  enu_of_ecef_point_d(&my_enu_point, &ltp_def, &my_ecef_point);

  printf("ecef to enu : (%f,%f,%f) -> (%f,%f,%f)\n",
	 my_ecef_point.x, my_ecef_point.y, my_ecef_point.z,
	 my_enu_point.x, my_enu_point.y, my_enu_point.z );
  printf("\n");
}

void dc_send_shot_position(void)
{
  int16_t phi = DegOfRad(estimator_phi*10.0f);
  int16_t theta = DegOfRad(estimator_theta*10.0f);
  float gps_z = ((float)gps.hmsl) / 1000.0f;
  int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));
  int16_t photo_nr = -1;

  if (dc_buffer < DC_IMAGE_BUFFER) {
    dc_buffer++;
    dc_photo_nr++;
    photo_nr = dc_photo_nr;
  }

  DOWNLINK_SEND_DC_SHOT(DefaultChannel,
                        &photo_nr,
                        &gps.utm_pos.east,
                        &gps.utm_pos.north,
                        &gps_z,
                        &gps.utm_pos.zone,
                        &phi,
                        &theta,
                        &course,
                        &gps.gspeed,
                        &gps.tow);
}

void dc_send_shot_position(void)
{
  // angles in decideg
  int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi*10.0f);
  int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta*10.0f);
  int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi*10.0f);
  // course in decideg
  int16_t course = DegOfRad(*stateGetHorizontalSpeedDir_f()) * 10;
  // ground speed in cm/s
  uint16_t speed = (*stateGetHorizontalSpeedNorm_f()) * 10;
  int16_t photo_nr = -1;

  if (dc_photo_nr < DC_IMAGE_BUFFER) {
    dc_photo_nr++;
    photo_nr = dc_photo_nr;
  }

  DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
                        &photo_nr,
                        &stateGetPositionLla_i()->lat,
                        &stateGetPositionLla_i()->lon,
                        &stateGetPositionLla_i()->alt,
                        &gps.hmsl,
                        &phi,
                        &theta,
                        &psi,
                        &course,
                        &speed,
                        &gps.tow);
}

static void send_cam(struct transport_tx *trans, struct link_device *dev)
{
  int16_t x = cam_target_x;
  int16_t y = cam_target_y;
  int16_t phi = DegOfRad(cam_phi_c);
  int16_t theta = DegOfRad(cam_theta_c);
  pprz_msg_send_CAM(trans, dev, AC_ID, &phi, &theta, &x, &y);
}

void sky_seg_avoid_run(void) {
  static int counter = 0;

  counter++;
  // Read Latest GST Module Results
  if (counter >= (512/15))
  {
    // Vertical
    float dx = sqrt ( (stateGetPositionEnu_f()->x * stateGetPositionEnu_f()->x) + (stateGetPositionEnu_f()->y * stateGetPositionEnu_f()->y) );

    float dh = (17.0f - stateGetPositionEnu_f()->z);
    float vang = atan2(dh,dx) * 80.9F * 2.0f; // 255 / PI

    if (vang < 0.0f)
      vang = 0.0f;
    if (vang > 255.0f)
      vang = 255.0f;

    // Horizontal
    float bearing = atan2(- stateGetPositionEnu_f()->x, - stateGetPositionEnu_f()->y );
    float heading = stateGetNedToBodyEulers_f()->psi;
    float diff = bearing - heading;
    NormAngleRad(diff);
    diff = DegOfRad(diff);

    float hang = DegOfRad(atan2(10,dx)); // 255 / PI


    float viewangle = 50; // degrees
    float range = viewangle/2.0f;
    float deg_per_bin = viewangle/ ((float) N_BINS);

    float bin_nr_mid = range/deg_per_bin;
    float bin_nr_start = bin_nr_mid + diff/deg_per_bin - hang/deg_per_bin;
    float bin_nr_stop  = bin_nr_mid + diff/deg_per_bin + hang/deg_per_bin;


    for (int i=0; i<N_BINS; i++)
    {
      if ((i > bin_nr_start) && (i <= (bin_nr_stop)))
      {
        gst2ppz.obstacle_bins[i] = vang;
      }
      else
      {
        gst2ppz.obstacle_bins[i] = 0.0f;
      }
    }

    counter = 0;
    run_avoid_navigation_onvision();
  }
}

gboolean timeout_callback(gpointer data) {

  for (int i=0; i<20; i++) {
    aos_compute_state();
    aos_compute_sensors();
#ifndef DISABLE_PROPAGATE
    ahrs_propagate();
#endif
#ifndef DISABLE_ACCEL_UPDATE
    ahrs_update_accel();
#endif
#ifndef DISABLE_MAG_UPDATE
    if (!(i%5)) ahrs_update_mag();
#endif
  }

#if AHRS_TYPE == AHRS_TYPE_ICE || AHRS_TYPE == AHRS_TYPE_ICQ
    EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_imu_euler, ahrs.ltp_to_imu_euler);
#endif

#if AHRS_TYPE == AHRS_TYPE_ICQ
    IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d", 
	       ahrs_impl.gyro_bias.p,
	       ahrs_impl.gyro_bias.q,
	       ahrs_impl.gyro_bias.r);
#endif
#if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2
    struct Int32Rates bias_i;
    RATES_BFP_OF_REAL(bias_i, ahrs_impl.gyro_bias);
    IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d", 
	       bias_i.p,
	       bias_i.q,
	       bias_i.r);
#endif

  IvySendMsg("183 AHRS_EULER %f %f %f", 
	     ahrs_float.ltp_to_imu_euler.phi,
	     ahrs_float.ltp_to_imu_euler.theta,
	     ahrs_float.ltp_to_imu_euler.psi);

  IvySendMsg("183 BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f",
	     DegOfRad(aos.imu_rates.p), 
	     DegOfRad(aos.imu_rates.q), 
	     DegOfRad(aos.imu_rates.r),
	     DegOfRad(aos.ltp_to_imu_euler.phi), 
	     DegOfRad(aos.ltp_to_imu_euler.theta),
	     DegOfRad(aos.ltp_to_imu_euler.psi));

  IvySendMsg("183 BOOZ_SIM_GYRO_BIAS %f %f %f",
	     DegOfRad(aos.gyro_bias.p), 
	     DegOfRad(aos.gyro_bias.q), 
	     DegOfRad(aos.gyro_bias.r));

  return TRUE;
}

value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, value cl, value t, value m, value lat, value lon) {
  gps_mode = (Bool_val(m) ? 3 : 0);
  gps_course = DegOfRad(Double_val(c)) * 10.;
  gps_alt = Double_val(a) * 100.;
  gps_gspeed = Double_val(s) * 100.;
  gps_climb = Double_val(cl) * 100.;
  gps_week = 0; // FIXME
  gps_itow = Double_val(t) * 1000.;

#ifdef GPS_USE_LATLONG
  gps_lat = DegOfRad(Double_val(lat))*1e7;
  gps_lon = DegOfRad(Double_val(lon))*1e7;
  latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0);
  gps_utm_east = latlong_utm_x * 100;
  gps_utm_north = latlong_utm_y * 100;
  gps_utm_zone = nav_utm_zone0;
  x = y = z; /* Just to get rid of the "unused arg" warning */
#else // GPS_USE_LATLONG
  gps_utm_east = Int_val(x);
  gps_utm_north = Int_val(y);
  gps_utm_zone = Int_val(z);
  lat = lon; /* Just to get rid of the "unused arg" warning */
#endif // GPS_USE_LATLONG


  /** Space vehicle info simulation */
  gps_nb_channels=7;
  int i;
  static int time;
  time++;
  for(i = 0; i < gps_nb_channels; i++) {
    gps_svinfos[i].svid = 7 + i;
    gps_svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45;
    gps_svinfos[i].azim = (time/gps_nb_channels + 50 * i) % 360;
    gps_svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.;
    gps_svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01);
    gps_svinfos[i].qi = (int)((time / 1000.) + i) % 8;
  }
  gps_PDOP = gps_Sacc = gps_Pacc = 500+200*sin(time/100.);
  gps_numSV = 7;

  gps_verbose_downlink = !launch;
  UseGpsPosNoSend(estimator_update_state_gps);
  gps_downlink();


  return Val_unit;
}

/** Reset the UTM zone to current GPS fix */
unit_t nav_reset_utm_zone(void) {

  struct UtmCoor_f utm0_old;
  utm0_old.zone = nav_utm_zone0;
  utm0_old.north = nav_utm_north0;
  utm0_old.east = nav_utm_east0;
  utm0_old.alt = ground_alt;
  struct LlaCoor_f lla0;
  lla_of_utm_f(&lla0, &utm0_old);

#ifdef GPS_USE_LATLONG
  /* Set the real UTM zone */
  nav_utm_zone0 = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
#else
  nav_utm_zone0 = gps.utm_pos.zone;
#endif

  struct UtmCoor_f utm0;
  utm0.zone = nav_utm_zone0;
  utm_of_lla_f(&utm0, &lla0);

  nav_utm_east0 = utm0.east;
  nav_utm_north0 = utm0.north;

  stateSetLocalUtmOrigin_f(&utm0);

  return 0;
}

uint8_t dc_info(void)
{
#ifdef DOWNLINK_SEND_DC_INFO
  float course = DegOfRad(stateGetNedToBodyEulers_f()->psi);
  int16_t mode = dc_autoshoot;
  uint8_t shutter = dc_autoshoot_period * 10;
  DOWNLINK_SEND_DC_INFO(DefaultChannel, DefaultDevice,
                        &mode,
                        &stateGetPositionLla_i()->lat,
                        &stateGetPositionLla_i()->lon,
                        &stateGetPositionLla_i()->alt,
                        &course,
                        &dc_photo_nr,
                        &dc_survey_interval,
                        &dc_gps_next_dist,
                        &dc_gps_x,
                        &dc_gps_y,
                        &dc_circle_start_angle,
                        &dc_circle_interval,
                        &dc_circle_last_block,
                        &dc_gps_count,
                        &shutter);
#endif
  return 0;
}

void mf_daq_send_report(void)
{
  // Send report over normal telemetry
  if (mf_daq.nb > 0) {
    DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 9, mf_daq.values);
  }
  // Test if log is started
  if (pprzLogFile != -1) {
    if (log_started == FALSE) {
      // Log MD5SUM once
      DOWNLINK_SEND_ALIVE(pprzlog_tp, chibios_sdlog, 16, MD5SUM);
      log_started = true;
    }
    // Log GPS for time reference
    uint8_t foo = 0;
    int16_t climb = -gps.ned_vel.z;
    int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6));
    struct UtmCoor_f utm = *stateGetPositionUtm_f();
    int32_t east = utm.east * 100;
    int32_t north = utm.north * 100;
    DOWNLINK_SEND_GPS(pprzlog_tp, chibios_sdlog, &gps.fix,
                      &east, &north, &course, &gps.hmsl, &gps.gspeed, &climb,
                      &gps.week, &gps.tow, &utm.zone, &foo);
  }
}

void aoa_pwm_update(void) {
  static float prev_aoa = 0.0f;

  // raw duty cycle in usec
  uint32_t duty_raw = get_pwm_input_duty_in_usec(AOA_PWM_CHANNEL);

  // remove some offset if needed
  aoa_pwm.raw = duty_raw - AOA_PWM_OFFSET;
  // FIXME for some reason, the last value of the MA3 encoder is not 4096 but 4097
  // this case is not handled since we don't care about angles close to +- 180 deg
  aoa_pwm.angle = AOA_SIGN * (((float)aoa_pwm.raw * aoa_pwm.sens) - aoa_pwm.offset - AOA_ANGLE_OFFSET);
  // filter angle
  aoa_pwm.angle = aoa_pwm.filter * prev_aoa + (1.0f - aoa_pwm.filter) * aoa_pwm.angle;
  prev_aoa = aoa_pwm.angle;

#if USE_AOA
  stateSetAngleOfAttack_f(aoa_adc.angle);
#endif

#if SEND_SYNC_AOA
  RunOnceEvery(10, DOWNLINK_SEND_AOA(DefaultChannel, DefaultDevice, &aoa_pwm.raw, &aoa_pwm.angle));
#endif

#if LOG_AOA
  if(pprzLogFile != -1) {
    if (!log_started) {
      sdLogWriteLog(pprzLogFile, "AOA_PWM: ANGLE(deg) RAW(int16)\n");
      log_started = true;
    } else {
      float angle = DegOfRad(aoa_pwm.angle);
      sdLogWriteLog(pprzLogFile, "AOA_PWM: %.3f %d\n", angle, aoa_pwm.raw);
    }
  }
#endif
}

bool_t disc_survey( uint8_t center, float radius) {
  float wind_dir = atan2(wind_north, wind_east) + M_PI;

  /** Not null even if wind_east=wind_north=0 */
  float upwind_x = cos(wind_dir);
  float upwind_y = sin(wind_dir);

  float grid = nav_survey_shift / 2;

  switch (status) {
  case UTURN:
    nav_circle_XY(c.x, c.y, grid*sign);
    if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
      c1.x = estimator_x;
      c1.y = estimator_y;

      float d = ScalarProduct(upwind_x, upwind_y, estimator_x-WaypointX(center), estimator_y-WaypointY(center));
      if (d > radius) {
	status = DOWNWIND;
      } else {
	float w = sqrt(radius*radius - d*d) - 1.5*grid;

	float crosswind_x = - upwind_y;
	float crosswind_y = upwind_x;

	c2.x = WaypointX(center)+d*upwind_x-w*sign*crosswind_x;
	c2.y = WaypointY(center)+d*upwind_y-w*sign*crosswind_y;

	status = SEGMENT;
      }
      nav_init_stage();
    }
    break;

  case DOWNWIND:
    c2.x = WaypointX(center) - upwind_x * radius;
    c2.y = WaypointY(center) - upwind_y * radius;
    status = SEGMENT;
    /* No break; */

  case SEGMENT:
    nav_route_xy(c1.x, c1.y, c2.x, c2.y);
    if (nav_approaching_xy(c2.x, c2.y, c1.x, c1.y, CARROT)) {
      c.x = c2.x + grid*upwind_x;
      c.y = c2.y + grid*upwind_y;

      sign = -sign;
      status = UTURN;
      nav_init_stage();
    }
    break;
  default:
    break;
  }

  NavVerticalAutoThrottleMode(0.); /* No pitch */
  NavVerticalAltitudeMode(WaypointAlt(center), 0.); /* No preclimb */

  return TRUE;
}

void ins_reset_local_origin( void ) {
#if INS_UPDATE_FW_ESTIMATOR
  struct UtmCoor_f utm;
#ifdef GPS_USE_LATLONG
  /* Recompute UTM coordinates in this zone */
  struct LlaCoor_f lla;
  lla.lat = gps.lla_pos.lat / 1e7;
  lla.lon = gps.lla_pos.lon / 1e7;
  utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
  utm_of_lla_f(&utm, &lla);
#else
  utm.zone = gps.utm_pos.zone;
  utm.east = gps.utm_pos.east / 100.0f;
  utm.north = gps.utm_pos.north / 100.0f;
#endif
  // ground_alt
  utm.alt = gps.hmsl / 1000.0f;
  // reset state UTM ref
  stateSetLocalUtmOrigin_f(&utm);
#else
  struct LtpDef_i ltp_def;
  ltp_def_from_ecef_i(&ltp_def, &gps.ecef_pos);
  ltp_def.hmsl = gps.hmsl;
  stateSetLocalOrigin_i(&ltp_def);
#endif
}

/** Reset the geographic reference to the current GPS fix */
unit_t nav_reset_reference( void ) {
#ifdef GPS_USE_LATLONG
  /* Set the real UTM zone */
  nav_utm_zone0 = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;

  /* Recompute UTM coordinates in this zone */
  struct LlaCoor_f lla;
  lla.lat = gps.lla_pos.lat/1e7;
  lla.lon = gps.lla_pos.lon/1e7;
  struct UtmCoor_f utm;
  utm.zone = nav_utm_zone0;
  utm_of_lla_f(&utm, &lla);
  nav_utm_east0 = utm.east;
  nav_utm_north0 = utm.north;
#else
  nav_utm_zone0 = gps.utm_pos.zone;
  nav_utm_east0 = gps.utm_pos.east/100;
  nav_utm_north0 = gps.utm_pos.north/100;
#endif

  // reset state UTM ref
  struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
  stateSetLocalUtmOrigin_f(&utm0);

  previous_ground_alt = ground_alt;
  ground_alt = gps.hmsl/1000.;
  return 0;
}

void ArduIMU_periodicGPS( void ) {

  if (ardu_gps_trans.status != I2CTransDone) { return; }

  // Test for high acceleration:
  //  - low speed
  //  - high thrust
  if (estimator_hspeed_dir < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
    high_accel_flag = TRUE;
  } else {
    high_accel_flag = FALSE;
    if (estimator_hspeed_dir > HIGH_ACCEL_LOW_SPEED && !high_accel_flag) {
      high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small)
    }
    if (estimator_hspeed_dir < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
      high_accel_done = FALSE; // Activate high accel after landing
    }
  }

  FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps.speed_3d); // speed 3D
  FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps.gspeed);   // ground speed
  FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)DegOfRad(gps.course / 1e6));   // course
  ardu_gps_trans.buf[12] = gps.fix;                              // status gps fix
  ardu_gps_trans.buf[13] = gps_ubx.status_flags;                      // status flags
  ardu_gps_trans.buf[14] = (uint8_t)high_accel_flag;              // high acceleration flag (disable accelerometers in the arduimu filter)
  I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 15);

}