/** 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(<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
}
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.;
}
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));
}
}
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);
}
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);
}
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.;
}
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;
}
static void update_state_interface(void)
{
// Send to Estimator (Control)
#ifdef XSENS_BACKWARDS
struct FloatEulers att = {
-xsens.euler.phi + ins_roll_neutral,
-xsens.euler.theta + ins_pitch_neutral,
xsens.euler.psi + RadOfDeg(180)
};
struct FloatEulerstRates rates = {
-xsens.gyro.p,
-xsens.gyro.q,
xsens.gyro.r
};
#else
struct FloatEulers att = {
xsens.euler.phi + ins_roll_neutral,
xsens.euler.theta + ins_pitch_neutral,
xsens.euler.psi
};
struct FloatRates rates = xsens.gyro;
#endif
stateSetNedToBodyEulers_f(&att);
stateSetBodyRates_f(&rates);
}
/** Navigation function along a path
*/
static inline bool_t mission_nav_path(struct _mission_element *el)
{
if (el->element.mission_path.nb == 0) {
return FALSE; // nothing to do
}
if (el->element.mission_path.path_idx == 0) { //first wp of path
el->element.mission_wp.wp.wp_i = el->element.mission_path.path.path_i[0];
if (!mission_nav_wp(el)) { el->element.mission_path.path_idx++; }
}
else if (el->element.mission_path.path_idx < el->element.mission_path.nb) { //standart wp of path
struct EnuCoor_i *from_wp = &(el->element.mission_path.path.path_i[(el->element.mission_path.path_idx) - 1]);
struct EnuCoor_i *to_wp = &(el->element.mission_path.path.path_i[el->element.mission_path.path_idx]);
//Check proximity and wait for t seconds in proximity circle if desired
if (nav_approaching_from(to_wp, from_wp, CARROT)) {
last_mission_wp = *to_wp;
if (el->duration > 0.) {
if (nav_check_wp_time(to_wp, el->duration)) {
el->element.mission_path.path_idx++;
}
} else { el->element.mission_path.path_idx++; }
}
//Route Between from-to
horizontal_mode = HORIZONTAL_MODE_ROUTE;
nav_route(from_wp, to_wp);
NavVerticalAutoThrottleMode(RadOfDeg(0.0));
NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(from_wp->z), 0.);
} else { return FALSE; } //end of path
return TRUE;
}
void ahrs_fc_update_gps(struct GpsState *gps_s)
{
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
if (gps_s->fix == GPS_FIX_3D) {
ahrs_fc.ltp_vel_norm = gps_s->speed_3d / 100.;
ahrs_fc.ltp_vel_norm_valid = TRUE;
} else {
ahrs_fc.ltp_vel_norm_valid = FALSE;
}
#endif
#if AHRS_USE_GPS_HEADING && USE_GPS
//got a 3d fix, ground speed > 0.5 m/s and course accuracy is better than 10deg
if (gps_s->fix == GPS_FIX_3D && gps_s->gspeed >= 500 &&
gps_s->cacc <= RadOfDeg(10 * 1e7)) {
// gps_s->course is in rad * 1e7, we need it in rad
float course = gps_s->course / 1e7;
if (ahrs_fc.heading_aligned) {
/* the assumption here is that there is no side-slip, so heading=course */
ahrs_fc_update_heading(course);
} else {
/* hard reset the heading if this is the first measurement */
ahrs_fc_realign_heading(course);
}
}
#endif
}
static void traj_sineX_quad_update(void)
{
const float om = RadOfDeg(10);
if (aos.time > (M_PI / om)) {
const float a = 20;
struct FloatVect3 jerk;
VECT3_ASSIGN(jerk , -a * om * om * om * cos(om * aos.time), 0, 0);
VECT3_ASSIGN(aos.ltp_accel , -a * om * om * sin(om * aos.time), 0, 0);
VECT3_ASSIGN(aos.ltp_vel , a * om * cos(om * aos.time), 0, 0);
VECT3_ASSIGN(aos.ltp_pos , a * sin(om * aos.time), 0, 0);
// this is based on differential flatness of the quad
EULERS_ASSIGN(aos.ltp_to_imu_euler, 0., atan2(aos.ltp_accel.x, 9.81), 0.);
float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);
const struct FloatEulers e_dot = {
0.,
9.81 * jerk.x / ((9.81 * 9.81) + (aos.ltp_accel.x * aos.ltp_accel.x)),
0.
};
FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);
}
}
static void traj_step_phi_update(void)
{
if (aos.time > 5) {
EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(5), 0, 0);
float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);
}
}
void estimator_update_state_gps( void ) {
float gps_east = gps_utm_east / 100.;
float gps_north = gps_utm_north / 100.;
/* Relative position to reference */
gps_east -= nav_utm_east0;
gps_north -= nav_utm_north0;
EstimatorSetPosXY(gps_east, gps_north);
#ifndef USE_BARO_ETS
float falt = gps_alt / 100.;
EstimatorSetAlt(falt);
#endif
float fspeed = gps_gspeed / 100.;
float fclimb = gps_climb / 100.;
float fcourse = RadOfDeg(gps_course / 10.);
EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
// Heading estimator from wind-information, usually computed with -DWIND_INFO
// wind_north and wind_east initialized to 0, so still correct if not updated
float w_vn = cosf(estimator_hspeed_dir) * estimator_hspeed_mod - wind_north;
float w_ve = sinf(estimator_hspeed_dir) * estimator_hspeed_mod - wind_east;
estimator_psi = atan2f(w_ve, w_vn);
if (estimator_psi < 0.)
estimator_psi += 2 * M_PI;
#ifdef EXTRA_DOWNLINK_DEVICE
DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta);
#endif
}
static inline void update_state_interface(void)
{
// Send to Estimator (Control)
#if XSENS_BACKWARDS
struct FloatEulers att = {
ins_phi + ins_roll_neutral,
-ins_theta + ins_pitch_neutral,
-ins_psi + RadOfDeg(180)
};
struct FloatRates rates = {
ins_p,
-ins_q,
-ins_r
};
#else
struct FloatEulers att = {
-ins_phi + ins_roll_neutral,
ins_theta + ins_pitch_neutral,
-ins_psi
};
struct FloatRates rates = {
-ins_p,
ins_q,
-ins_r
};
#endif
stateSetNedToBodyEulers_f(&att);
stateSetBodyRates_f(&rates);
}
static void traj_bungee_takeoff_init(void)
{
aos.traj->te = 40.;
EULERS_ASSIGN(aos.ltp_to_imu_euler, 0, RadOfDeg(10), 0);
float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);
}
static void test_5(void) {
struct FloatEulers fe;
EULERS_ASSIGN(fe, RadOfDeg(-10.66), RadOfDeg(-0.7), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("fe", fe);
printf("\n");
struct FloatQuat fq;
FLOAT_QUAT_OF_EULERS(fq, fe);
test_eulers_of_quat(fq, 1);
printf("\n");
struct FloatRMat frm;
FLOAT_RMAT_OF_EULERS(frm, fe);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("frm", frm);
test_eulers_of_rmat(frm, 1);
printf("\n");
}
int main(int argc, char **argv)
{
// Set the default tracking system position and angle
struct LlaCoor_d tracking_lla;
tracking_lla.lat = RadOfDeg(51.9906340);
tracking_lla.lon = RadOfDeg(4.3767889);
tracking_lla.alt = 45.103;
tracking_offset_angle = 33.0 / 57.6;
ltp_def_from_lla_d(&tracking_ltp, &tracking_lla);
// 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;
}
