• 设为首页
  • 点击收藏
  • 手机版
    手机扫一扫访问
    迪恩网络手机版
  • 关注官方公众号
    微信扫一扫关注
    公众号
登陆 注册

C++ RadOfDeg函数代码示例

原作者: [db:作者] 来自: [db:来源] 收藏 邀请

本文整理汇总了C++中RadOfDeg函数的典型用法代码示例。如果您正苦于以下问题：C++ RadOfDeg函数的具体用法？C++ RadOfDeg怎么用？C++ RadOfDeg使用的例子？那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。



在下文中一共展示了RadOfDeg函数的20个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于我们的系统推荐出更棒的C++代码示例。

示例1: ir_init

/** 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;
}
开发者ID:BackupTheBerlios，项目名称:freedomflies-svn，代码行数:38，代码来源:papp_ir.c


示例2: test1234

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

}
开发者ID:EwoudSmeur，项目名称:paparazzi，代码行数:27，代码来源:test_algebra.c


示例3: test_10

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

}
开发者ID:EwoudSmeur，项目名称:paparazzi，代码行数:29，代码来源:test_algebra.c


示例4: test_10

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

}
开发者ID:byrddev，项目名称:paparazzi，代码行数:28，代码来源:test_algebra.c


示例5: test_9

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);
}
开发者ID:byrddev，项目名称:paparazzi，代码行数:7，代码来源:test_algebra.c


示例6: init_ltp

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

}
开发者ID:2seasuav，项目名称:paparuzzi，代码行数:30，代码来源:nps_fdm_crrcsim.c


示例7: test1234

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




}
开发者ID:byrddev，项目名称:paparazzi，代码行数:29，代码来源:test_algebra.c


示例8: nav_catapult

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()
开发者ID:glason，项目名称:paparazzi，代码行数:58，代码来源:nav_catapult.c


示例9: nav_catapult

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;

}
开发者ID:WenFly123，项目名称:openPPZ，代码行数:58，代码来源:nav_catapult.c


示例10: traj_static_sine_update

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

}
开发者ID:AshuLara，项目名称:paparazzi，代码行数:10，代码来源:ahrs_on_synth.c


示例11: test_of_axis_angle

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

}
开发者ID:EwoudSmeur，项目名称:paparazzi，代码行数:55，代码来源:test_algebra.c


示例12: InitializeZHAWSkidLanding

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;
}
开发者ID:Bruzzlee，项目名称:paparazzi，代码行数:55，代码来源:ZHAWNav_Landing.c


示例13: traj_static_sine_update

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

}
开发者ID:EwoudSmeur，项目名称:paparazzi，代码行数:11，代码来源:ahrs_on_synth.c


示例14: ins_vectornav_yaw_pitch_roll_to_attitude

/**
 * 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;
}
开发者ID:MosesWangHao，项目名称:paparazzi，代码行数:17，代码来源:ins_vectornav.c


示例15: test_quat2

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

}
开发者ID:byrddev，项目名称:paparazzi，代码行数:52，代码来源:test_algebra.c


示例16: gps_parse

void gps_parse(void) {
  if (gps_network == NULL) return;

  //Read from the network
  int size = network_read( gps_network, &gps_udp_read_buffer[0], 256);

  if(size > 0)
  {
    // Correct MSG
    if ((size == GPS_UDP_MSG_LEN) && (gps_udp_read_buffer[0] == STX))
    {
      gps.lla_pos.lat = RadOfDeg(UDP_GPS_INT(gps_udp_read_buffer+4));
      gps.lla_pos.lon = RadOfDeg(UDP_GPS_INT(gps_udp_read_buffer+8));
      gps.lla_pos.alt = UDP_GPS_INT(gps_udp_read_buffer+12);
      gps.hmsl        = UDP_GPS_INT(gps_udp_read_buffer+16);

      gps.ecef_pos.x = UDP_GPS_INT(gps_udp_read_buffer+20);
      gps.ecef_pos.y = UDP_GPS_INT(gps_udp_read_buffer+24);
      gps.ecef_pos.z = UDP_GPS_INT(gps_udp_read_buffer+28);

      gps.ecef_vel.x = UDP_GPS_INT(gps_udp_read_buffer+32);
      gps.ecef_vel.y = UDP_GPS_INT(gps_udp_read_buffer+36);
      gps.ecef_vel.z = UDP_GPS_INT(gps_udp_read_buffer+40);

      gps.fix = GPS_FIX_3D;
      gps_available = TRUE;

#if GPS_USE_LATLONG
      // Computes from (lat, long) in the referenced UTM zone
      struct LlaCoor_f lla_f;
      lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
      lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
      struct UtmCoor_f utm_f;
      utm_f.zone = nav_utm_zone0;
      // convert to utm
      utm_of_lla_f(&utm_f, &lla_f);
      // copy results of utm conversion
      gps.utm_pos.east = utm_f.east*100;
      gps.utm_pos.north = utm_f.north*100;
      gps.utm_pos.alt = gps.lla_pos.alt;
      gps.utm_pos.zone = nav_utm_zone0;
#endif

    }
    else
    {
      printf("gps_udp error: msg len invalid %d bytes\n",size);
    }
    memset(&gps_udp_read_buffer[0], 0, sizeof(gps_udp_read_buffer));
  }
}
开发者ID:1bitsquared，项目名称:paparazzi，代码行数:51，代码来源:gps_udp.c


示例17: test_2

static void test_2(void)
{

  struct Int32Vect3 v1 = { 5000, 5000, 5000 };
  DISPLAY_INT32_VECT3("v1", v1);

  struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)};
  DISPLAY_FLOAT_EULERS("euler_f", euler_f);

  struct Int32Eulers euler_i;
  EULERS_BFP_OF_REAL(euler_i, euler_f);
  DISPLAY_INT32_EULERS("euler_i", euler_i);

  struct Int32Quat quat_i;
  int32_quat_of_eulers(&quat_i, &euler_i);
  DISPLAY_INT32_QUAT("quat_i", quat_i);
  int32_quat_normalize(&quat_i);
  DISPLAY_INT32_QUAT("quat_i_n", quat_i);

  struct Int32Vect3 v2;
  int32_quat_vmult(&v2, &quat_i, &v1);
  DISPLAY_INT32_VECT3("v2", v2);

  struct Int32RMat rmat_i;
  int32_rmat_of_quat(&rmat_i, &quat_i);
  DISPLAY_INT32_RMAT("rmat_i", rmat_i);

  struct Int32Vect3 v3;
  INT32_RMAT_VMULT(v3, rmat_i, v1);
  DISPLAY_INT32_VECT3("v3", v3);

  struct Int32RMat rmat_i2;
  int32_rmat_of_eulers(&rmat_i2, &euler_i);
  DISPLAY_INT32_RMAT("rmat_i2", rmat_i2);

  struct Int32Vect3 v4;
  INT32_RMAT_VMULT(v4, rmat_i2, v1);
  DISPLAY_INT32_VECT3("v4", v4);

  struct FloatQuat quat_f;
  float_quat_of_eulers(&quat_f, &euler_f);
  DISPLAY_FLOAT_QUAT("quat_f", quat_f);

  struct FloatVect3 v5;
  VECT3_COPY(v5, v1);
  DISPLAY_FLOAT_VECT3("v5", v5);
  struct FloatVect3 v6;
  float_quat_vmult(&v6, &quat_f, &v5);
  DISPLAY_FLOAT_VECT3("v6", v6);

}
开发者ID:EwoudSmeur，项目名称:paparazzi，代码行数:51，代码来源:test_algebra.c


示例18: test_2

static void test_2(void) {

    struct Int32Vect3 v1 = { 5000, 5000, 5000 };
    DISPLAY_INT32_VECT3("v1", v1);

    struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)};
    DISPLAY_FLOAT_EULERS("euler_f", euler_f);

    struct Int32Eulers euler_i;
    EULERS_BFP_OF_REAL(euler_i, euler_f);
    DISPLAY_INT32_EULERS("euler_i", euler_i);

    struct Int32Quat quat_i;
    INT32_QUAT_OF_EULERS(quat_i, euler_i);
    DISPLAY_INT32_QUAT("quat_i", quat_i);
    INT32_QUAT_NORMALIZE(quat_i);
    DISPLAY_INT32_QUAT("quat_i_n", quat_i);

    struct Int32Vect3 v2;
    INT32_QUAT_VMULT(v2, quat_i, v1);
    DISPLAY_INT32_VECT3("v2", v2);

    struct Int32RMat rmat_i;
    INT32_RMAT_OF_QUAT(rmat_i, quat_i);
    DISPLAY_INT32_RMAT("rmat_i", rmat_i);

    struct Int32Vect3 v3;
    INT32_RMAT_VMULT(v3, rmat_i, v1);
    DISPLAY_INT32_VECT3("v3", v3);

    struct Int32RMat rmat_i2;
    INT32_RMAT_OF_EULERS(rmat_i2, euler_i);
    DISPLAY_INT32_RMAT("rmat_i2", rmat_i2);

    struct Int32Vect3 v4;
    INT32_RMAT_VMULT(v4, rmat_i2, v1);
    DISPLAY_INT32_VECT3("v4", v4);

    struct FloatQuat quat_f;
    FLOAT_QUAT_OF_EULERS(quat_f, euler_f);
    DISPLAY_FLOAT_QUAT("quat_f", quat_f);

    struct FloatVect3 v5;
    VECT3_COPY(v5, v1);
    DISPLAY_FLOAT_VECT3("v5", v5);
    struct FloatVect3 v6;
    FLOAT_QUAT_VMULT(v6, quat_f, v5);
    DISPLAY_FLOAT_VECT3("v6", v6);

}
开发者ID:byrddev，项目名称:paparazzi，代码行数:50，代码来源:test_algebra.c


示例19: test_quat2

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

}
开发者ID:EwoudSmeur，项目名称:paparazzi，代码行数:50，代码来源:test_algebra.c


示例20: sim_use_gps_pos

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;
}
开发者ID:MarkGriffin，项目名称:paparazzi，代码行数:48，代码来源:sim_gps.c



注：本文中的RadOfDeg函数示例由纯净天空整理自Github/MSDocs等源码及文档管理平台，相关代码片段筛选自各路编程大神贡献的开源项目，源码版权归原作者所有，传播和使用请参考对应项目的License；未经允许，请勿转载。

鲜花
握手
雷人
路过
鸡蛋
该文章已有0人参与评论

请发表评论

全部评论

专题导读
More+
上一篇：
C++ Radian函数代码示例发布时间：2022-05-30
下一篇：
C++ R_useDynamicSymbols函数代码示例发布时间：2022-05-30
热门推荐
More+
热门话题
More+
阅读排行榜

扫描微信二维码

查看手机版网站

随时了解更新最新资讯

139-2527-9053

在线客服（服务时间 9:00～18:00）

在线QQ客服
地址：深圳市南山区西丽大学城创智工业园
电邮：jeky_zhao#qq.com
移动电话：139-2527-9053

Powered by 互联科技 X3.4© 2001-2213 极客世界.|Sitemap