本文整理汇总了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(<pdef, &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(); //获得x坐标
WaypointY(_to) = GetPosY(); //获得y坐标
WaypointAlt(_to) = GetPosAlt(); //获得高度
nav_catapult_x = stateGetPositionEnu_f()->x; //起飞点x坐标
nav_catapult_y = stateGetPositionEnu_f()->y; //起飞点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;未经允许,请勿转载。 |
请发表评论