C++ RadOfDeg函数代码示例

/** Initialize \a ir with the \a IR_DEFAULT_CONTRAST \n
 *  Initialize \a adc_buf_channel
 */
void ir_init(void) {
  RadOfIrFromContrast(IR_DEFAULT_CONTRAST);
  adc_buf_channel(ADC_CHANNEL_IR1, &buf_ir1, ADC_CHANNEL_IR_NB_SAMPLES);
  adc_buf_channel(ADC_CHANNEL_IR2, &buf_ir2, ADC_CHANNEL_IR_NB_SAMPLES);
#ifdef ADC_CHANNEL_IR_TOP
  adc_buf_channel(ADC_CHANNEL_IR_TOP, &buf_ir_top, ADC_CHANNEL_IR_NB_SAMPLES);
  z_contrast_mode = Z_CONTRAST_DEFAULT;
#else
  z_contrast_mode = 0;
#endif
 
  ir_roll_neutral  = RadOfDeg(IR_ROLL_NEUTRAL_DEFAULT);
  ir_pitch_neutral = RadOfDeg(IR_PITCH_NEUTRAL_DEFAULT);

  estimator_rad_of_ir = ir_rad_of_ir;

#if defined IR_CORRECTION_LEFT && defined IR_CORRECTION_RIGHT
  ir_correction_left = IR_CORRECTION_LEFT;
  ir_correction_right = IR_CORRECTION_RIGHT;
#endif

#if defined IR_CORRECTION_UP && defined IR_CORRECTION_DOWN
  ir_correction_up = IR_CORRECTION_UP;
  ir_correction_down = IR_CORRECTION_DOWN;
#endif

  ir_360 = TRUE;
  ir_estimated_phi_pi_4 = IR_ESTIMATED_PHI_PI_4;
  ir_estimated_phi_minus_pi_4 = IR_ESTIMATED_PHI_MINUS_PI_4;
  ir_estimated_theta_pi_4 = IR_ESTIMATED_THETA_PI_4;
  ir_estimated_theta_minus_pi_4 = IR_ESTIMATED_THETA_MINUS_PI_4;

  ir_contrast = IR_DEFAULT_CONTRAST;
  ir_rad_of_ir = IR_RAD_OF_IR_CONTRAST / IR_DEFAULT_CONTRAST;
}

void test1234(void)
{
  struct FloatEulers eul = {RadOfDeg(33.), RadOfDeg(25.), RadOfDeg(26.)};

  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_tmp;
  float_rmat_comp(&r_tmp, &r_yaw, &r_roll);

  struct FloatRMat r_att;
  float_rmat_comp(&r_att, &r_tmp, &r_pitch);
  DISPLAY_FLOAT_RMAT("r_att_ref  ", r_att);

  float_rmat_of_eulers_312(&r_att, &eul);
  DISPLAY_FLOAT_RMAT("r_att312  ", r_att);

}

static void test_10(void)
{

  struct FloatEulers euler;
  EULERS_ASSIGN(euler , RadOfDeg(0.), RadOfDeg(10.), RadOfDeg(0.));
  DISPLAY_FLOAT_EULERS_DEG("euler", euler);
  struct FloatQuat quat;
  float_quat_of_eulers(&quat, &euler);
  DISPLAY_FLOAT_QUAT("####quat", quat);

  struct Int32Eulers euleri;
  EULERS_BFP_OF_REAL(euleri, euler);
  DISPLAY_INT32_EULERS("euleri", euleri);
  struct Int32Quat quati;
  int32_quat_of_eulers(&quati, &euleri);
  DISPLAY_INT32_QUAT("####quat", quati);
  struct Int32RMat rmati;
  int32_rmat_of_eulers(&rmati, &euleri);
  DISPLAY_INT32_RMAT("####rmat", rmati);

  struct Int32Quat quat_ltp_to_body;
  struct Int32Quat body_to_imu_quat;
  int32_quat_identity(&body_to_imu_quat);


  int32_quat_comp_inv(&quat_ltp_to_body, &body_to_imu_quat, &quati);
  DISPLAY_INT32_QUAT("####quat_ltp_to_body", quat_ltp_to_body);

}

static void test_10(void) {

    struct FloatEulers euler;
    EULERS_ASSIGN(euler , RadOfDeg(0.), RadOfDeg(10.), RadOfDeg(0.));
    DISPLAY_FLOAT_EULERS_DEG("euler", euler);
    struct FloatQuat quat;
    FLOAT_QUAT_OF_EULERS(quat, euler);
    DISPLAY_FLOAT_QUAT("####quat", quat);

    struct Int32Eulers euleri;
    EULERS_BFP_OF_REAL(euleri, euler);
    DISPLAY_INT32_EULERS("euleri", euleri);
    struct Int32Quat quati;
    INT32_QUAT_OF_EULERS(quati, euleri);
    DISPLAY_INT32_QUAT("####quat", quati);
    struct Int32RMat rmati;
    INT32_RMAT_OF_EULERS(rmati, euleri);
    DISPLAY_INT32_RMAT("####rmat", rmati);

    struct Int32Quat quat_ltp_to_body;
    struct Int32Quat body_to_imu_quat;
    INT32_QUAT_ZERO( body_to_imu_quat);


    INT32_QUAT_COMP_INV(quat_ltp_to_body, body_to_imu_quat, quati);
    DISPLAY_INT32_QUAT("####quat_ltp_to_body", quat_ltp_to_body);

}

static void test_9(void) {
    struct FloatEulers eul;
    eul.phi   = RadOfDeg(80.821376);
    eul.theta = RadOfDeg(44.227319);
    eul.psi   = RadOfDeg(-134.047298);
    float err = test_INT32_QUAT_OF_RMAT(&eul, TRUE);
}

static void init_ltp(void)
{

  struct LlaCoor_d llh_nav0; /* Height above the ellipsoid */
  llh_nav0.lat = RadOfDeg((double)NAV_LAT0 / 1e7);
  llh_nav0.lon = RadOfDeg((double)NAV_LON0 / 1e7);
  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
  llh_nav0.alt = (NAV_ALT0 + NAV_MSL0) / 1000.;

  struct EcefCoor_d ecef_nav0;
  ecef_of_lla_d(&ecef_nav0, &llh_nav0);

  ltp_def_from_ecef_d(&ltpdef, &ecef_nav0);

  fdm.ltp_g.x = 0.;
  fdm.ltp_g.y = 0.;
  fdm.ltp_g.z = 0.; // accel data are already with the correct format

#ifdef AHRS_H_X
#pragma message "Using magnetic field as defined in airframe file."
  fdm.ltp_h.x = AHRS_H_X;
  fdm.ltp_h.y = AHRS_H_Y;
  fdm.ltp_h.z = AHRS_H_Z;
#else
  fdm.ltp_h.x = 0.4912;
  fdm.ltp_h.y = 0.1225;
  fdm.ltp_h.z = 0.8624;
#endif

}

void test1234(void) {
    struct FloatEulers eul = {RadOfDeg(33.), RadOfDeg(25.), RadOfDeg(26.)};

    struct FloatVect3 uz = { 0., 0., 1.};
    struct FloatMat33 r_yaw;
    FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi);

    struct FloatVect3 uy = { 0., 1., 0.};
    struct FloatMat33 r_pitch;
    FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta);

    struct FloatVect3 ux = { 1., 0., 0.};
    struct FloatMat33 r_roll;
    FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi);

    struct FloatMat33 r_tmp;
    FLOAT_RMAT_COMP(r_tmp, r_yaw, r_roll);

    struct FloatMat33 r_att;
    FLOAT_RMAT_COMP(r_att, r_tmp, r_pitch);
    DISPLAY_FLOAT_RMAT("r_att_ref  ", r_att);

    FLOAT_RMAT_OF_EULERS_312(r_att, eul);
    DISPLAY_FLOAT_RMAT("r_att312  ", r_att);




}

bool_t nav_catapult(uint8_t _to, uint8_t _climb)
{
  float alt = WaypointAlt(_climb);

  nav_catapult_armed = 1;

  // No Roll, Climb Pitch, No motor Phase
  if (nav_catapult_launch <= nav_catapult_motor_delay)
  {
    NavAttitude(RadOfDeg(0));
    NavVerticalAutoThrottleMode(nav_catapult_initial_pitch);
    NavVerticalThrottleMode(9600*(0));



    // Store take-off waypoint
    WaypointX(_to) = GetPosX();
    WaypointY(_to) = GetPosY();
    WaypointAlt(_to) = GetPosAlt();

    nav_catapult_x = stateGetPositionEnu_f()->x;
    nav_catapult_y = stateGetPositionEnu_f()->y;

  }
  // No Roll, Climb Pitch, Full Power
  else if (nav_catapult_launch < nav_catapult_heading_delay)
  {
    NavAttitude(RadOfDeg(0));
    NavVerticalAutoThrottleMode(nav_catapult_initial_pitch);
    NavVerticalThrottleMode(9600*(nav_catapult_initial_throttle));
  }
  // Normal Climb: Heading Locked by Waypoint Target
  else if (nav_catapult_launch == 0xffff)
  {
    NavVerticalAltitudeMode(alt, 0);	// vertical mode (folow glideslope)
    NavVerticalAutoThrottleMode(0);		// throttle mode
    NavGotoWaypoint(_climb);				// horizontal mode (stay on localiser)
  }
  else
  {
    // Store Heading, move Climb
    nav_catapult_launch = 0xffff;

    float dir_x = stateGetPositionEnu_f()->x - nav_catapult_x;
    float dir_y = stateGetPositionEnu_f()->y - nav_catapult_y;

    float dir_L = sqrt(dir_x * dir_x + dir_y * dir_y);

    WaypointX(_climb) = nav_catapult_x + (dir_x / dir_L) * 300;
    WaypointY(_climb) = nav_catapult_y + (dir_y / dir_L) * 300;

    DownlinkSendWp(DefaultChannel, DefaultDevice, _climb);
  }


return TRUE;

}	// end of gls()

bool_t nav_catapult(uint8_t _to, uint8_t _climb)
{
  float alt = WaypointAlt(_climb);  

  nav_catapult_armed = 1;

  // No Roll, Climb Pitch, No motor Phase   零滚转 府仰爬行 没有电机阶段
  if (nav_catapult_launch <= nav_catapult_motor_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ)
  {
    NavAttitude(RadOfDeg(0));  //高度设置
    NavVerticalAutoThrottleMode(nav_catapult_initial_pitch);   //自动油门模式
    NavVerticalThrottleMode(9600*(0));   //设定油门


    // Store take-off waypoint   存储起飞点
    WaypointX(_to) = GetPosX();   //获得ｘ坐标
    WaypointY(_to) = GetPosY();   //获得ｙ坐标
    WaypointAlt(_to) = GetPosAlt();   //获得高度

    nav_catapult_x = stateGetPositionEnu_f()->x;   //起飞点ｘ坐标
    nav_catapult_y = stateGetPositionEnu_f()->y;   //起飞点ｙ坐标

  }
  // No Roll, Climb Pitch, Full Power   零滚转  府仰爬行  满油门
  else if (nav_catapult_launch < nav_catapult_heading_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ)
  {
    NavAttitude(RadOfDeg(0));   //高度设置
    NavVerticalAutoThrottleMode(nav_catapult_initial_pitch);   //自动油门模式
    NavVerticalThrottleMode(9600*(nav_catapult_initial_throttle));   //设定油门
  }
  // Normal Climb: Heading Locked by Waypoint Target    
  // 正常爬行：锁定给定航点
  else if (nav_catapult_launch == 0xffff)
  {
    NavVerticalAltitudeMode(alt, 0);	// vertical mode (folow glideslope)  水平模式（跟随滑坡）
    NavVerticalAutoThrottleMode(0);		// throttle mode  油门模式
    NavGotoWaypoint(_climb);				// horizontal mode (stay on localiser)   垂直模式（保持定位）
  }
  else
  {
    // Store Heading, move Climb   
    nav_catapult_launch = 0xffff;

    float dir_x = stateGetPositionEnu_f()->x - nav_catapult_x;
    float dir_y = stateGetPositionEnu_f()->y - nav_catapult_y;

    float dir_L = sqrt(dir_x * dir_x + dir_y * dir_y);

    WaypointX(_climb) = nav_catapult_x + (dir_x / dir_L) * 300;
    WaypointY(_climb) = nav_catapult_y + (dir_y / dir_L) * 300;

    DownlinkSendWp(DefaultChannel, DefaultDevice, _climb);
  }


return TRUE;

}

static void traj_static_sine_update(void) {
  

  aos.imu_rates.p = RadOfDeg(200)*cos(aos.time);
  aos.imu_rates.q = RadOfDeg(200)*cos(0.7*aos.time+2);
  aos.imu_rates.r = RadOfDeg(200)*cos(0.8*aos.time+1);
  FLOAT_QUAT_INTEGRATE(aos.ltp_to_imu_quat, aos.imu_rates, aos.dt);
  FLOAT_EULERS_OF_QUAT(aos.ltp_to_imu_euler, aos.ltp_to_imu_quat);

}

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

}

bool_t InitializeZHAWSkidLanding(uint8_t AFWP, uint8_t TDWP, uint8_t CPWP, float radius)
{
	AFWaypoint = AFWP;
	TDWaypoint = TDWP;
	CPWaypoint = CPWP;
	
	CLandingStatus = CircleDown;
	LandRadius = radius;
	LandAppAlt = estimator_z;
	TDDistance = fabs(Landing_TDDistance); 		//TDDistance can only be positive
	SonarHeight = fabs(Landing_SonarHeight);	//SonarHeight can only be positive

	/* 
	// für Airframe****************
	SaveHeight = 3;			//Höhe für Failsave
	SonarHeight = 6;		//Höhe für Approach
	TDDistance = 50;		//Strecke, auf der geflaret wird
	FlareFactor = 0.5;		//Faktor mit dem die Sollhöhe beim Flare verkleinert wird
	// ******************************** 
	*/

	set_approach_params();  // Parameter für Landung setzten (Airspeed, max_roll, ...)
	set_as_mode(3);		// Airspeed Pitch Simple
		
	//Translate distance from AF to TD so that AF is (0/0) 
	float x_0 = waypoints[TDWaypoint].x - waypoints[AFWaypoint].x;
	float y_0 = waypoints[TDWaypoint].y - waypoints[AFWaypoint].y;

	// Unit vector from AF to TD
	float d = sqrt(x_0*x_0+y_0*y_0);	//d=Horizontale Strecke von AF zu TD
	float x_1 = x_0 / d;
	float y_1 = y_0 / d;

	//find the center of LandCircle
	LandCircle.x = waypoints[AFWaypoint].x + y_1 * LandRadius;
	LandCircle.y = waypoints[AFWaypoint].y - x_1 * LandRadius;


	//Compute the QDR-Angles
	LandCircleQDR = atan2(waypoints[AFWaypoint].x-LandCircle.x, waypoints[AFWaypoint].y-LandCircle.y);

	if(LandRadius > 0)
	{
		ApproachQDR = LandCircleQDR-RadOfDeg(90);
		LandCircleQDR = LandCircleQDR-RadOfDeg(45);
	}
	else
	{
		ApproachQDR = LandCircleQDR+RadOfDeg(90);
		LandCircleQDR = LandCircleQDR+RadOfDeg(45);
	}
	

	return FALSE;
}

static void traj_static_sine_update(void)
{


  aos.imu_rates.p = RadOfDeg(200) * cos(aos.time);
  aos.imu_rates.q = RadOfDeg(200) * cos(0.7 * aos.time + 2);
  aos.imu_rates.r = RadOfDeg(200) * cos(0.8 * aos.time + 1);
  float_quat_integrate(&aos.ltp_to_imu_quat, &aos.imu_rates, aos.dt);
  float_eulers_of_quat(&aos.ltp_to_imu_euler, &aos.ltp_to_imu_quat);

}

/**
 * Convert yaw, pitch, and roll data from VectorNav
 * to correct attitude
 * yaw(0), pitch(1), roll(2) -> phi, theta, psi
 * [deg] -> rad
 */
void ins_vectornav_yaw_pitch_roll_to_attitude(struct FloatEulers *vn_attitude)
{
  static struct FloatEulers att_rad;
  att_rad.phi = RadOfDeg(vn_attitude->psi);
  att_rad.theta = RadOfDeg(vn_attitude->theta);
  att_rad.psi = RadOfDeg(vn_attitude->phi);

  vn_attitude->phi = att_rad.phi;
  vn_attitude->theta = att_rad.theta;
  vn_attitude->psi = att_rad.psi;
}

float test_quat2(void) {

    struct FloatEulers eula2b;
    EULERS_ASSIGN(eula2b, RadOfDeg(70.), RadOfDeg(0.), RadOfDeg(0.));
    //  DISPLAY_FLOAT_EULERS_DEG("eula2b", eula2b);

    struct FloatQuat qa2b;
    FLOAT_QUAT_OF_EULERS(qa2b, eula2b);
    DISPLAY_FLOAT_QUAT("qa2b", qa2b);

    struct DoubleEulers eula2b_d;
    EULERS_ASSIGN(eula2b_d, RadOfDeg(70.), RadOfDeg(0.), RadOfDeg(0.));
    struct DoubleQuat qa2b_d;
    DOUBLE_QUAT_OF_EULERS(qa2b_d, eula2b_d);
    DISPLAY_FLOAT_QUAT("qa2b_d", qa2b_d);

    struct FloatVect3 u = { 1., 0., 0.};
    float angle = RadOfDeg(70.);

    struct FloatQuat q;
    FLOAT_QUAT_OF_AXIS_ANGLE(q, u, angle);
    DISPLAY_FLOAT_QUAT("q ", q);





    struct FloatEulers eula2c;
    EULERS_ASSIGN(eula2c, RadOfDeg(80.), RadOfDeg(0.), RadOfDeg(0.));
    //  DISPLAY_FLOAT_EULERS_DEG("eula2c", eula2c);

    struct FloatQuat qa2c;
    FLOAT_QUAT_OF_EULERS(qa2c, eula2c);
    DISPLAY_FLOAT_QUAT("qa2c", qa2c);


    struct FloatQuat qb2a;
    FLOAT_QUAT_INVERT(qb2a, qa2b);
    DISPLAY_FLOAT_QUAT("qb2a", qb2a);


    struct FloatQuat qb2c1;
    FLOAT_QUAT_COMP(qb2c1, qb2a, qa2c);
    DISPLAY_FLOAT_QUAT("qb2c1", qb2c1);

    struct FloatQuat qb2c2;
    FLOAT_QUAT_INV_COMP(qb2c2, qa2b, qa2c);
    DISPLAY_FLOAT_QUAT("qb2c2", qb2c2);

    return 0.;

}