自学教程：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);}

bool_t snav_circle1(void) {  /* circle around CD until QDR_TD */  NavVerticalAutoThrottleMode(0); /* No pitch */  NavVerticalAltitudeMode(wp_cd.a, 0.);  nav_circle_XY(wp_cd.x, wp_cd.y, d_radius);  return(! NavQdrCloseTo(DegOfRad(qdr_td)));}

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;  }#if DC_SHOT_EXTRA_DL  // send a message on second datalink first  // (for instance an embedded CPU)  DOWNLINK_SEND_DC_SHOT(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,                        &photo_nr,                        &stateGetPositionLla_i()->lat,                        &stateGetPositionLla_i()->lon,                        &stateGetPositionLla_i()->alt,                        &gps.hmsl,                        &phi,                        &theta,                        &psi,                        &course,                        &speed,                        &gps.tow);#endif  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);}

int main(int argc, char **argv){  // Set the default tracking system position and angle  struct EcefCoor_d tracking_ecef;  //alt 45 m because of ellipsoid altitude in Delft  tracking_ecef.x = 3924331.5;  tracking_ecef.y = 300361.7;  tracking_ecef.z = 5002197.1;  tracking_offset_angle = 33.0 / 57.6;  ltp_def_from_ecef_d(&tracking_ltp, &tracking_ecef);  // Parse the options from cmdline  parse_options(argc, argv);  printf_debug("Tracking system Latitude: %f Longitude: %f Offset to North: %f degrees/n", DegOfRad(tracking_ltp.lla.lat),               DegOfRad(tracking_ltp.lla.lon), DegOfRad(tracking_offset_angle));  // Create the network connections  printf_debug("Starting NatNet listening (multicast address: %s, data port: %d, version: %d.%d)/n",               natnet_multicast_addr, natnet_data_port, natnet_major, natnet_minor);  udp_socket_create(&natnet_data, "", -1, natnet_data_port, 0); // Only receiving  udp_socket_subscribe_multicast(&natnet_data, natnet_multicast_addr);  udp_socket_set_recvbuf(&natnet_data, 0x100000); // 1MB  printf_debug("Starting NatNet command socket (server address: %s, command port: %d)/n", natnet_addr, natnet_cmd_port);  udp_socket_create(&natnet_cmd, natnet_addr, natnet_cmd_port, 0, 1);  udp_socket_set_recvbuf(&natnet_cmd, 0x100000); // 1MB  // Create the Ivy Client  GMainLoop *ml =  g_main_loop_new(NULL, FALSE);  IvyInit("natnet2ivy", "natnet2ivy READY", 0, 0, 0, 0);  IvyStart(ivy_bus);  // Create the main timers  printf_debug("Starting transmitting and sampling timeouts (transmitting frequency: %dHz, minimum velocity samples: %d)/n",               freq_transmit, min_velocity_samples);  g_timeout_add(1000 / freq_transmit, timeout_transmit_callback, NULL);  GIOChannel *sk = g_io_channel_unix_new(natnet_data.sockfd);  g_io_add_watch(sk, G_IO_IN | G_IO_NVAL | G_IO_HUP,                 sample_data, NULL);  // Run the main loop  g_main_loop_run(ml);  return 0;}

bool snav_circle2(void){  /* circle around CA until QDR_A */  NavVerticalAutoThrottleMode(0); /* No pitch */  NavVerticalAltitudeMode(wp_cd.a, 0.);  nav_circle_XY(wp_ca.x, wp_ca.y, a_radius);  return (! NavQdrCloseTo(DegOfRad(qdr_a)));}

void test_of_axis_angle(void){  struct FloatVect3 axis = { 0., 1., 0.};  FLOAT_VECT3_NORMALIZE(axis);  DISPLAY_FLOAT_VECT3("axis", axis);  const float angle = RadOfDeg(30.);  printf("angle %f/n", DegOfRad(angle));  struct FloatQuat my_q;  FLOAT_QUAT_OF_AXIS_ANGLE(my_q, axis, angle);  DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("quat", my_q);  struct FloatRMat my_r1;  float_rmat_of_quat(&my_r1, &my_q);  DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", my_r1);  DISPLAY_FLOAT_RMAT("rmat1", my_r1);  struct FloatRMat my_r;  FLOAT_RMAT_OF_AXIS_ANGLE(my_r, axis, angle);  DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", my_r);  DISPLAY_FLOAT_RMAT("rmat", my_r);  printf("/n");  struct FloatEulers eul = {RadOfDeg(30.), RadOfDeg(30.), 0.};  struct FloatVect3 uz = { 0., 0., 1.};  struct FloatRMat r_yaw;  FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi);  struct FloatVect3 uy = { 0., 1., 0.};  struct FloatRMat r_pitch;  FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta);  struct FloatVect3 ux = { 1., 0., 0.};  struct FloatRMat r_roll;  FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi);  struct FloatRMat r_yaw_pitch;  float_rmat_comp(&r_yaw_pitch, &r_yaw, &r_pitch);  struct FloatRMat r_yaw_pitch_roll;  float_rmat_comp(&r_yaw_pitch_roll, &r_yaw_pitch, &r_roll);  DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", r_yaw_pitch_roll);  DISPLAY_FLOAT_RMAT("rmat", r_yaw_pitch_roll);  DISPLAY_FLOAT_EULERS_DEG("eul", eul);  struct FloatRMat rmat1;  float_rmat_of_eulers(&rmat1, &eul);  DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", rmat1);  DISPLAY_FLOAT_RMAT("rmat1", rmat1);}

static void send_gps_lla(struct transport_tx *trans, struct link_device *dev){  uint8_t err = 0;  int16_t climb = -gps.ned_vel.z;  int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6));  pprz_msg_send_GPS_LLA(trans, dev, AC_ID,                        &gps.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt,                        &gps.hmsl, &course, &gps.gspeed, &climb,                        &gps.week, &gps.tow,                        &gps.fix, &err);}

static void send_gps(struct transport_tx *trans, struct link_device *dev){  uint8_t zero = 0;  int16_t climb = -gps.ned_vel.z;  int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6));  pprz_msg_send_GPS(trans, dev, AC_ID, &gps.fix,                    &gps.utm_pos.east, &gps.utm_pos.north,                    &course, &gps.hmsl, &gps.gspeed, &climb,                    &gps.week, &gps.tow, &gps.utm_pos.zone, &zero);  // send SVINFO for available satellites that have new data  send_svinfo_available(trans, dev);}

/* shoot on circle */uint8_t dc_circle(float interval, float start) {  dc_autoshoot = DC_AUTOSHOOT_CIRCLE;  dc_gps_count = 0;  dc_circle_interval = fmodf(fmaxf(interval, 1.), 360.);  if(start == DC_IGNORE) {    start = DegOfRad(stateGetNedToBodyEulers_f()->psi);  }  dc_circle_start_angle = fmodf(start, 360.);  if (start < 0.)    start += 360.;  //dc_circle_last_block = floorf(dc_circle_start_angle/dc_circle_interval);  dc_circle_last_block = 0;  dc_circle_max_blocks = floorf(360./dc_circle_interval);  dc_info();  return 0;}

/** * Send Metrics typically displayed on a HUD for fixed wing aircraft. */static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device *dev){  /* Current heading in degrees, in compass units (0..360, 0=north) */  int16_t heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);  /* Current throttle setting in integer percent, 0 to 100 */  // is a 16bit unsigned int but supposed to be from 0 to 100??  uint16_t throttle;#ifdef COMMAND_THRUST  throttle = commands[COMMAND_THRUST] / (MAX_PPRZ / 100);#elif defined COMMAND_THROTTLE  throttle = commands[COMMAND_THROTTLE] / (MAX_PPRZ / 100);#endif  mavlink_msg_vfr_hud_send(MAVLINK_COMM_0,                           stateGetAirspeed_f(),                           stateGetHorizontalSpeedNorm_f(), // groundspeed                           heading,                           throttle,                           stateGetPositionLla_f()->alt, // altitude, FIXME: should be MSL                           stateGetSpeedNed_f()->z); // climb rate  MAVLinkSendMessage();}

static void mavlink_send_gps_raw_int(struct transport_tx *trans, struct link_device *dev){#if USE_GPS  int32_t course = DegOfRad(gps.course) / 1e5;  if (course < 0) {    course += 36000;  }  mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,                               get_sys_time_usec(),                               gps.fix,                               gps.lla_pos.lat,                               gps.lla_pos.lon,                               gps.lla_pos.alt,                               gps.pdop,                               UINT16_MAX, // VDOP                               gps.gspeed,                               course,                               gps.num_sv);  MAVLinkSendMessage();#endif}

static void mavlink_send_global_position_int(struct transport_tx *trans, struct link_device *dev){  float heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);  if (heading < 0.) {    heading += 360;  }  uint16_t compass_heading = heading * 100;  int32_t relative_alt = stateGetPositionLla_i()->alt - state.ned_origin_f.hmsl;  /// TODO: check/ask what coordinate system vel is supposed to be in, not clear from docs  mavlink_msg_global_position_int_send(MAVLINK_COMM_0,                                       get_sys_time_msec(),                                       stateGetPositionLla_i()->lat,                                       stateGetPositionLla_i()->lon,                                       stateGetPositionLla_i()->alt,                                       relative_alt,                                       stateGetSpeedNed_f()->x * 100,                                       stateGetSpeedNed_f()->y * 100,                                       stateGetSpeedNed_f()->z * 100,                                       compass_heading);  MAVLinkSendMessage();}

uint8_t dc_info(void) {#ifdef DOWNLINK_SEND_DC_INFO  float course = DegOfRad(estimator_psi);  DOWNLINK_SEND_DC_INFO(DefaultChannel,                        &dc_autoshoot,                        &estimator_x,                        &estimator_y,                        &course,                        &dc_buffer,                        &dc_gps_dist,                        &dc_gps_next_dist,                        &dc_gps_x,                        &dc_gps_y,                        &dc_circle_start_angle,                        &dc_circle_interval,                        &dc_circle_last_block,                        &dc_gps_count,                        &dc_autoshoot_quartersec_period);#endif  return 0;}

/** Reset the geographic reference to the current GPS fix */void ins_reset_local_origin(void) {    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);    // reset filter flag    ins_impl.reset_alt_ref = TRUE;}

static void test_enu_of_ecef_int(void) {  printf("/n--- enu_of_ecef int ---/n");  struct EcefCoor_f ref_coor_f = { 4624497.0 , 116475.0, 4376563.0};  struct LtpDef_f ltp_def_f;  ltp_def_from_ecef_f(&ltp_def_f, &ref_coor_f);  struct EcefCoor_i ref_coor_i = { rint(CM_OF_M(ref_coor_f.x)),				   rint(CM_OF_M(ref_coor_f.y)),				   rint(CM_OF_M(ref_coor_f.z))};  printf("ecef0 : (%d,%d,%d)/n", ref_coor_i.x, ref_coor_i.y, ref_coor_i.z);  struct LtpDef_i ltp_def_i;  ltp_def_from_ecef_i(&ltp_def_i, &ref_coor_i);  printf("lla0 : (%d %d %d) (%f,%f,%f)/n", ltp_def_i.lla.lat, ltp_def_i.lla.lon, ltp_def_i.lla.alt,	 DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lat)),	 DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lon)),	 M_OF_CM((double)ltp_def_i.lla.alt));#define STEP    1000.#define RANGE 100000.  double sum_err = 0;  struct FloatVect3 max_err;  FLOAT_VECT3_ZERO(max_err);  struct FloatVect3 offset;  for (offset.x=-RANGE; offset.x<=RANGE; offset.x+=STEP) {    for (offset.y=-RANGE; offset.y<=RANGE; offset.y+=STEP) {      for (offset.z=-RANGE; offset.z<=RANGE; offset.z+=STEP) {	struct EcefCoor_f my_ecef_point_f = ref_coor_f;	VECT3_ADD(my_ecef_point_f, offset);	struct EnuCoor_f  my_enu_point_f;	enu_of_ecef_point_f(&my_enu_point_f, &ltp_def_f, &my_ecef_point_f);#if DEBUG	printf("ecef to enu float : (%.02f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)/n",	       my_ecef_point_f.x, my_ecef_point_f.y, my_ecef_point_f.z,	       my_enu_point_f.x, my_enu_point_f.y, my_enu_point_f.z );#endif	struct EcefCoor_i my_ecef_point_i = { rint(CM_OF_M(my_ecef_point_f.x)),					      rint(CM_OF_M(my_ecef_point_f.y)),					      rint(CM_OF_M(my_ecef_point_f.z))};;	struct EnuCoor_i  my_enu_point_i;	enu_of_ecef_point_i(&my_enu_point_i, &ltp_def_i, &my_ecef_point_i);#if DEBUG	//	printf("def->ecef (%d,%d,%d)/n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z);	printf("ecef to enu int   : (%.2f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)/n/n",	       M_OF_CM((double)my_ecef_point_i.x),	       M_OF_CM((double)my_ecef_point_i.y),	       M_OF_CM((double)my_ecef_point_i.z),	       M_OF_CM((double)my_enu_point_i.x),	       M_OF_CM((double)my_enu_point_i.y),	       M_OF_CM((double)my_enu_point_i.z));#endif	float ex = my_enu_point_f.x - M_OF_CM((double)my_enu_point_i.x);	if (fabs(ex) > max_err.x) max_err.x = fabs(ex);	float ey = my_enu_point_f.y - M_OF_CM((double)my_enu_point_i.y);	if (fabs(ey) > max_err.y) max_err.y = fabs(ey);	float ez = my_enu_point_f.z - M_OF_CM((double)my_enu_point_i.z);	if (fabs(ez) > max_err.z) max_err.z = fabs(ez);	sum_err += ex*ex + ey*ey + ez*ez;      }    }  }  double nb_samples = (2*RANGE / STEP + 1) * (2*RANGE / STEP + 1) *  (2*RANGE / STEP + 1);  printf("enu_of_ecef int/float comparison:/n");  printf("error max (%f,%f,%f) m/n", max_err.x, max_err.y, max_err.z );  printf("error avg (%f ) m /n", sqrt(sum_err) / nb_samples );  printf("/n");}