// NOTE: The spatial_ahrs_* functions were largely copied from Madgwick's
// Quaternion implementation of the 'DCM filter'
// https://code.google.com/p/imumargalgorithm30042010sohm/
void spatial_ahrs_init(SpatialInfo *spatial_info,
double ax, double ay, double az,
double mx, double my, double mz) {
double xaxis[3] = {1.0, 0.0, 0.0};
double yaxis[3] = {0.0, 1.0, 0.0};
double zaxis[3] = {0.0, 0.0, 1.0};
// We'll assume the current state by way of the accelerometer and compass readings:
double dOrientEuler[3];
spatial_orientation_from_accel_and_compass((double *)&dOrientEuler, ax, ay, az, mx, my, mz);
// Now we construct a Roll & Pitch Quat from the Euler's
float fRollQ[4];
quat_from_axis_and_angle((double *) &xaxis, dOrientEuler[1], (float *) &fRollQ);
float fPitchQ[4];
quat_from_axis_and_angle((double *) &yaxis, dOrientEuler[0], (float *) &fPitchQ);
float fRollPitchQ[4];
quat_mult((float *) &fRollQ, (float *) &fPitchQ, (float *)&fRollPitchQ);
// Rotate the the Quat around our 'Z' so that we're facing North:
// NOTE, Due to the madgwick origin, -0.5*M_PI is North, and we add the
// bearing offset to this angle
double bearing_in_rad = 2*M_PI+dOrientEuler[2];
float fCompassQ[4];
quat_from_axis_and_angle((double *) &zaxis, bearing_in_rad, (float *) &fCompassQ);
// Commit this to the persisting orientation state:
quat_mult((float *) &fRollPitchQ, (float *) &fCompassQ, (float *) spatial_info->orientation_q);
spatial_info->is_ahrs_initialized = true;
}
void quat_rotate(vec3_t *res, const quat_t *q, const vec3_t *v)
{
quat_t tq, itq, tv;
quat_conj(&itq, q);
tv = QUAT(0, *v);
quat_mult(&tq, q, &tv);
quat_mult(&tq, &tq, &itq);
*res = tq.v;
}
// q_out = qa^-1 * qb
void
quat_inv_mult(quat_t *q_out, const quat_t * const qa, const quat_t * const qb)
{
quat_t qa_inv;
quat_inv(&qa_inv,qa);
quat_mult(q_out, &qa_inv, qb);
}
static void step(void *void_data, double time) {
(void) time;
struct cube_data *data = (struct cube_data*) void_data;
data->q = quat_mult(data->rot, data->q);
quat_to_matrix(data->q, data->m);
}
// q_out = qa * qb^-1
void
quat_mult_inv(quat_t *q_out, const quat_t * const qa, const quat_t * const qb)
{
quat_t qb_inv;
quat_inv(&qb_inv,qb);
quat_mult(q_out, qa, &qb_inv);
}
// get "heading" from attitude quat q_n2b:
// first find the shortest path to vertical (q_n2h),
// then find how much rotation q_n2h has relative to north
static double
get_heading_from_q_n2b(const quat_t * const q_n2b)
{
double R13 = 2*( -q_n2b->q0*q_n2b->q2 + q_n2b->q1*q_n2b->q3 );
double R23 = 2*( q_n2b->q0*q_n2b->q1 + q_n2b->q2*q_n2b->q3 );
double R33 = -1 + 2*q_n2b->q0*q_n2b->q0 + 2*q_n2b->q3*q_n2b->q3;
// get body to heading quat q_n2h
if (fabs(R13) >= 1.0)
R13 /= R13 + 1.0e-12;
double theta = acos(-R13);
double norm_rb = sqrt( R23*R23 + R33*R33 ) + 1e-12;
quat_t q_b2h = { cos(0.5*theta),
0.0,
R33/norm_rb*sin(0.5*theta),
-R23/norm_rb*sin(0.5*theta)};
quat_t q_n2h;
quat_mult( &q_n2h, q_n2b, &q_b2h );
quat_t q_y90 = {sqrt(2.0)/2.0, 0.0, sqrt(2.0)/2.0, 0.0};
quat_t q_theta;
quat_inv_mult( &q_theta, &q_y90, &q_n2h );
if ( fabs(q_theta.q0) > 1.0)
q_theta.q0 /= q_theta.q0 + 1.0e-12;
double heading = -2*acos(q_theta.q0);
if (q_theta.q1 < 0.0)
heading = -heading;
return heading;
}
quat_t
quat_mult_ret(const quat_t qa, const quat_t qb)
{
quat_t quat_out;
quat_mult(&quat_out, &qa, &qb);
return quat_out;
}
/**
* quat_rotate_rev:
* @rot: Unit quaternion that specifies the rotation.
* @v: 3-vector that is rotated according to the quaternion @rot
* and modified in place with the result.
*
* Rotates a vector @v from one coordinate system to another as
* specified by a unit quaternion @rot, but performs the rotation
* in the reverse direction of quat_rotate().
*/
void
quat_rotate_rev (const double * rot, double * v)
{
double a[4], b[4], c[4];
b[0] = 0;
memcpy (b+1, v, 3 * sizeof (double));
quat_mult (a, b, rot);
b[0] = rot[0];
b[1] = -rot[1];
b[2] = -rot[2];
b[3] = -rot[3];
quat_mult (c, b, a);
memcpy (v, c+1, 3 * sizeof (double));
}
void quat_concat(gdouble *q, gdouble *v, gdouble a)
{
gdouble ha, sa, qr[4];
/* create and apply the desired quaternion rotation */
ha = 0.5*a;
sa = sin(ha);
VEC4SET(qr, cos(ha), v[0]*sa, v[1]*sa, v[2]*sa);
quat_mult(q, qr);
}
/*
The Madwick algorithm :
* Uses +x to indicate North, we want +y which means we need a -90 degree rotation around z.
* Uses the following coordinate system:
http://stackoverflow.com/questions/17788043/madgwicks-sensor-fusion-algorithm-on-ios
*/
void spatial_madgeq_to_openglq(float *fMadgQ, float *fRetQ) {
float fTmpQ[4];
float fXYRotationQ[4] = { sqrt(0.5), 0, 0, -1.0*sqrt(0.5) }; // wxyz
fTmpQ[0] = fMadgQ[0];
fTmpQ[1] = fMadgQ[1] * -1.0f;
fTmpQ[2] = fMadgQ[2];
fTmpQ[3] = fMadgQ[3] * -1.0f;
// And then store the Rotation into fTranslatedQ
quat_mult((float *) &fTmpQ, (float *) &fXYRotationQ, fRetQ);
}
// ===========================================================================================
void Quaternion_byAngleAndVector(quat_t *Q, const real_t q_angle, const vec3_t *q_vector)
{
vec3_t rotation_axis;
normRv(&rotation_axis, q_vector);
//real_t f = _sin(q_angle * 0.5);
real_t f, cs;
_sin_cos(q_angle*0.5, &f, &cs);
vec3_copy(&Q->v, &rotation_axis);
vec3_mult(&Q->v, f);
//Q->s = _cos(q_angle*0.5);
Q->s = cs;
quat_mult(Q, 1.0 / quat_norm(Q));
}
int
quaternion_test()
{
#define FAIL_TEST { fprintf(stderr, "quaternion_test failed at line %d\n", \
__LINE__ ); return 0; }
fprintf(stderr, "running quaternion test\n");
double theta = 0;
double rvec[] = { 0, 0, 1 };
double q[4];
double roll, pitch, yaw;
quat_from_angle_axis( theta, rvec, q );
if( ! qeq( q, 1, 0, 0, 0 ) ) FAIL_TEST;
quat_to_roll_pitch_yaw( q, &roll, &pitch, &yaw );
if( ! rpyeq( roll,pitch,yaw, 0,0,0 ) ) FAIL_TEST;
// quat_from_angle_axis
theta = M_PI;
quat_from_angle_axis( theta, rvec, q );
fprintf(stderr,"<%.3f, %.3f, %.3f, %.3f>\n", q[0], q[1], q[2], q[3]);
if( ! qeq( q, 0, 0, 0, 1 ) ) FAIL_TEST;
// quat_to_angle_axis
quat_to_angle_axis( q, &theta, rvec );
if( !feq( theta, M_PI ) ) FAIL_TEST;
if( !feq(rvec[0], 0) || !feq(rvec[1], 0) || !feq(rvec[2], 1) ) FAIL_TEST;
quat_to_roll_pitch_yaw( q, &roll, &pitch, &yaw );
if( ! rpyeq( roll,pitch,yaw, 0,0,M_PI ) ) FAIL_TEST;
double q2[4];
double q3[4];
double axis1[] = { 0, 1, 0 };
double axis2[] = { 0, 0, 1 };
quat_from_angle_axis( M_PI/2, axis1, q );
quat_from_angle_axis( M_PI/2, axis2, q2 );
quat_mult( q3, q, q2 );
rvec[0] = 0; rvec[1] = 0; rvec[2] = 1;
quat_rotate( q, rvec );
fprintf(stderr, "by q: [ %.2f, %.2f, %.2f ]\n", rvec[0], rvec[1], rvec[2]);
rvec[0] = 0; rvec[1] = 0; rvec[2] = 1;
quat_rotate( q2, rvec );
fprintf(stderr, "by q2: [ %.2f, %.2f, %.2f ]\n", rvec[0], rvec[1], rvec[2]);
rvec[0] = 0; rvec[1] = 0; rvec[2] = 1;
quat_rotate( q3, rvec );
fprintf(stderr, "by q*q2: [ %.2f, %.2f, %.2f ]\n",
rvec[0], rvec[1], rvec[2]);
rvec[0] = 0; rvec[1] = 0; rvec[2] = 1;
quat_mult( q3, q2, q );
quat_rotate( q3, rvec );
fprintf(stderr, "by q2*q: [ %.2f, %.2f, %.2f ]\n",
rvec[0], rvec[1], rvec[2]);
// TODO
#undef FAIL_TEST
fprintf(stderr, "quaternion_test complete\n");
return 1;
}
/**
* Module task
*/
static void stabilizationTask(void* parameters)
{
portTickType lastSysTime;
portTickType thisSysTime;
UAVObjEvent ev;
ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired;
RateDesiredData rateDesired;
AttitudeActualData attitudeActual;
AttitudeRawData attitudeRaw;
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
SettingsUpdatedCb((UAVObjEvent *) NULL);
// Main task loop
lastSysTime = xTaskGetTickCount();
ZeroPids();
while(1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
{
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
continue;
}
// Check how long since last update
thisSysTime = xTaskGetTickCount();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);
AttitudeActualGet(&attitudeActual);
AttitudeRawGet(&attitudeRaw);
RateDesiredGet(&rateDesired);
SystemSettingsGet(&systemSettings);
#if defined(PIOS_QUATERNION_STABILIZATION)
// Quaternion calculation of error in each axis. Uses more memory.
float rpy_desired[3];
float q_desired[4];
float q_error[4];
float local_error[3];
// Essentially zero errors for anything in rate or none
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[0] = stabDesired.Roll;
else
rpy_desired[0] = attitudeActual.Roll;
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[1] = stabDesired.Pitch;
else
rpy_desired[1] = attitudeActual.Pitch;
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[2] = stabDesired.Yaw;
else
rpy_desired[2] = attitudeActual.Yaw;
RPY2Quaternion(rpy_desired, q_desired);
quat_inverse(q_desired);
quat_mult(q_desired, &attitudeActual.q1, q_error);
quat_inverse(q_error);
Quaternion2RPY(q_error, local_error);
#else
// Simpler algorithm for CC, less memory
float local_error[3] = {stabDesired.Roll - attitudeActual.Roll,
stabDesired.Pitch - attitudeActual.Pitch,
stabDesired.Yaw - attitudeActual.Yaw};
local_error[2] = fmod(local_error[2] + 180, 360) - 180;
#endif
for(uint8_t i = 0; i < MAX_AXES; i++) {
gyro_filtered[i] = gyro_filtered[i] * gyro_alpha + attitudeRaw.gyros[i] * (1 - gyro_alpha);
}
float *attitudeDesiredAxis = &stabDesired.Roll;
float *actuatorDesiredAxis = &actuatorDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll;
//Calculate desired rate
for(uint8_t i=0; i< MAX_AXES; i++)
{
switch(stabDesired.StabilizationMode[i])
{
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
rateDesiredAxis[i] = attitudeDesiredAxis[i];
axis_lock_accum[i] = 0;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
{
float weak_leveling = local_error[i] * weak_leveling_kp;
if(weak_leveling > weak_leveling_max)
weak_leveling = weak_leveling_max;
if(weak_leveling < -weak_leveling_max)
weak_leveling = -weak_leveling_max;
rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling;
axis_lock_accum[i] = 0;
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i]);
axis_lock_accum[i] = 0;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
if(fabs(attitudeDesiredAxis[i]) > max_axislock_rate) {
// While getting strong commands act like rate mode
rateDesiredAxis[i] = attitudeDesiredAxis[i];
axis_lock_accum[i] = 0;
} else {
// For weaker commands or no command simply attitude lock (almost) on no gyro change
axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT;
if(axis_lock_accum[i] > max_axis_lock)
axis_lock_accum[i] = max_axis_lock;
else if(axis_lock_accum[i] < -max_axis_lock)
axis_lock_accum[i] = -max_axis_lock;
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i]);
}
break;
}
}
uint8_t shouldUpdate = 1;
RateDesiredSet(&rateDesired);
ActuatorDesiredGet(&actuatorDesired);
//Calculate desired command
for(int8_t ct=0; ct< MAX_AXES; ct++)
{
if(rateDesiredAxis[ct] > settings.MaximumRate[ct])
rateDesiredAxis[ct] = settings.MaximumRate[ct];
else if(rateDesiredAxis[ct] < -settings.MaximumRate[ct])
rateDesiredAxis[ct] = -settings.MaximumRate[ct];
switch(stabDesired.StabilizationMode[ct])
{
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
{
float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct] - gyro_filtered[ct]);
actuatorDesiredAxis[ct] = bound(command);
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
switch (ct)
{
case ROLL:
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);
shouldUpdate = 1;
break;
case PITCH:
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);
shouldUpdate = 1;
break;
case YAW:
actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);
shouldUpdate = 1;
break;
}
break;
}
}
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL)
shouldUpdate = 0;
if(shouldUpdate)
{
actuatorDesired.Throttle = stabDesired.Throttle;
if(dT > 15)
actuatorDesired.NumLongUpdates++;
ActuatorDesiredSet(&actuatorDesired);
}
if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
(lowThrottleZeroIntegral && stabDesired.Throttle < 0) ||
!shouldUpdate)
{
ZeroPids();
}
// Clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}
void
toytronics_set_sp_incremental_from_rc()
{
double dt = 1.0/RC_UPDATE_FREQ;
const rc_t * const rc = get_rc();
const quat_t * const q_n2b = get_q_n2b();
// local copies to allow implementing a deadband
double rcp = apply_deadband(rc->pitch, SETPOINT_DEADBAND);
double rcr = apply_deadband(rc->roll, SETPOINT_DEADBAND);
double rcy = apply_deadband(rc->yaw, SETPOINT_DEADBAND);
// rotation vector in body frame
xyz_t w_dt_body = {rcr * SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*dt,
rcp * SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*dt,
rcy * SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*dt};
// try accelerometer turn coordination
w_dt_body.z -= dt*tc_fading(q_n2b)*aerobatic_accel_tc_gain*get_y_accel();
// old body to setpoint quat q_b2sp
quat_inv_mult( &(setpoint.q_b2sp), q_n2b, &(setpoint.q_n2sp));
// rotation vector in setpoint frame
xyz_t w_dt_sp;
rot_vec_by_quat_a2b( &w_dt_sp, &(setpoint.q_b2sp), &w_dt_body);
// form diff quat
double total_angle = sqrt( w_dt_sp.x*w_dt_sp.x
+ w_dt_sp.y*w_dt_sp.y
+ w_dt_sp.z*w_dt_sp.z
+ 1e-9);
quat_t diff_quat = {cos(total_angle/2.0),
sin(total_angle/2.0)*w_dt_sp.x/total_angle,
sin(total_angle/2.0)*w_dt_sp.y/total_angle,
sin(total_angle/2.0)*w_dt_sp.z/total_angle};
// use diff quat to update setpoint quat
setpoint.q_n2sp = quat_mult_ret(setpoint.q_n2sp, diff_quat);
// calculate body to setpoint quat
quat_inv_mult( &(setpoint.q_b2sp), q_n2b, &(setpoint.q_n2sp));
// now bound setpoint quat to not get too far away from estimated quat
BOUND(setpoint.q_b2sp.q1, -setpoint_incremental_bounds_deg.x*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.x*M_PI/180.0/2.0);
BOUND(setpoint.q_b2sp.q2, -setpoint_incremental_bounds_deg.y*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.y*M_PI/180.0/2.0);
BOUND(setpoint.q_b2sp.q3, -setpoint_incremental_bounds_deg.z*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.z*M_PI/180.0/2.0);
// let setpoint decay back to body
discrete_exponential_decay( &setpoint.q_b2sp.q1, setpoint_aerobatic_decay_time.x, dt );
discrete_exponential_decay( &setpoint.q_b2sp.q2, setpoint_aerobatic_decay_time.y, dt );
discrete_exponential_decay( &setpoint.q_b2sp.q3, setpoint_aerobatic_decay_time.z, dt );
// normalize
setpoint.q_b2sp.q0 = sqrt(1 - SQR(setpoint.q_b2sp.q1) - SQR(setpoint.q_b2sp.q2) - SQR(setpoint.q_b2sp.q3));
// update n2sp quat
quat_mult( &(setpoint.q_n2sp), q_n2b, &(setpoint.q_b2sp));
// set stabilization setpoint
set_stabilization_setpoint(&setpoint.q_n2sp);
}
void
toytronics_set_sp_hover_forward_from_rc()
{
double dt = 1.0/RC_UPDATE_FREQ;
const rc_t * const rc = get_rc();
const quat_t * const q_n2b = get_q_n2b();
// estimated heading for telemetry and bounding
setpoint.estimated_heading = hover_forward_yaw_of_quat( q_n2b );
// local copies to allow implementing a deadband
double rcp = rc->pitch;
double rcr = rc->roll;
double rcy = apply_deadband(rc->yaw, SETPOINT_DEADBAND);
// set pitch/yaw from stick
double pitch_body = (rcp * SETPOINT_MAX_STICK_ANGLE_DEG + hover_pitch_trim_deg)*M_PI/180.0;
double roll_body = rcr * SETPOINT_MAX_STICK_ANGLE_DEG*M_PI/180.0;
// integrate stick to get setpoint heading
setpoint.setpoint_heading += dt*SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*rcy;
// body roll feedforward turn coordination
#define START_FADING_DEG -50
#define FINISH_FADING_DEG -70
double ff_fading_slider;
if (pitch_body > START_FADING_DEG*M_PI/180.0)
ff_fading_slider = 0;
else if (pitch_body < FINISH_FADING_DEG*M_PI/180.0)
ff_fading_slider = 1;
else
ff_fading_slider = (pitch_body*180.0/M_PI - START_FADING_DEG)/(FINISH_FADING_DEG - START_FADING_DEG);
setpoint.setpoint_heading += dt*roll_body*roll_to_yaw_rate_ff_factor*ff_fading_slider;
// accel y turn coordination
setpoint.setpoint_heading -= dt*tc_fading(q_n2b)*hover_forward_accel_tc_gain*get_y_accel();
// bound heading error
double heading_error = setpoint.setpoint_heading - setpoint.estimated_heading;
wrap_to_pi(&heading_error);
BOUND(heading_error, -setpoint_absolute_heading_bound_deg*M_PI/180.0, setpoint_absolute_heading_bound_deg*M_PI/180.0);
setpoint.setpoint_heading = setpoint.estimated_heading + heading_error;
wrap_to_pi(&setpoint.setpoint_heading);
// start straight up
quat_t q_y90 = {sqrt(2)/2.0, 0, sqrt(2)/2.0, 0};
quat_memcpy( &setpoint.q_n2sp, &q_y90 );
// yaw
quat_t q_yaw = {1,0,0,0};
q_yaw.q0 = cos(0.5*setpoint.setpoint_heading);
q_yaw.q1 = -sin(0.5*setpoint.setpoint_heading);
quat_t q_temp;
quat_memcpy( &q_temp, &setpoint.q_n2sp );
quat_mult( &setpoint.q_n2sp, &q_temp, &q_yaw );
// roll
quat_t q_roll = {1,0,0,0};
q_roll.q0 = cos(0.5*roll_body);
q_roll.q3 = sin(0.5*roll_body);
quat_memcpy( &q_temp, &setpoint.q_n2sp );
quat_mult( &setpoint.q_n2sp, &q_temp, &q_roll );
// pitch
quat_t q_pitch = {1,0,0,0};
q_pitch.q0 = cos(0.5*pitch_body);
q_pitch.q2 = sin(0.5*pitch_body);
quat_memcpy( &q_temp, &setpoint.q_n2sp );
quat_mult( &setpoint.q_n2sp, &q_temp, &q_pitch );
/* // calculate body to setpoint quat */
/* quat_inv_mult( &(setpoint.q_b2sp), q_n2b, &(setpoint.q_n2sp)); */
/* // now bound setpoint quat to not get too far away from estimated quat */
/* BOUND(setpoint.q_b2sp.q1, -setpoint_incremental_bounds_deg.x*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.x*M_PI/180.0/2.0); */
/* BOUND(setpoint.q_b2sp.q2, -setpoint_incremental_bounds_deg.y*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.y*M_PI/180.0/2.0); */
/* BOUND(setpoint.q_b2sp.q3, -setpoint_incremental_bounds_deg.z*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.z*M_PI/180.0/2.0); */
/* // let setpoint decay back to body */
/* discrete_exponential_decay( &setpoint.q_b2sp.q1, setpoint_aerobatic_decay_time.x, dt ); */
/* discrete_exponential_decay( &setpoint.q_b2sp.q2, setpoint_aerobatic_decay_time.y, dt ); */
/* discrete_exponential_decay( &setpoint.q_b2sp.q3, setpoint_aerobatic_decay_time.z, dt ); */
/* // normalize */
/* setpoint.q_b2sp.q0 = sqrt(1 - SQR(setpoint.q_b2sp.q1) - SQR(setpoint.q_b2sp.q2) - SQR(setpoint.q_b2sp.q3)); */
/* // update n2sp quat */
/* quat_mult( &(setpoint.q_n2sp), q_n2b, &(setpoint.q_b2sp)); */
// update setpoint.heading
setpoint.setpoint_heading = hover_forward_yaw_of_quat( &setpoint.q_n2sp );
// set stabilization setpoint
set_stabilization_setpoint(&setpoint.q_n2sp);
}
void pure_nav_attup(void)
{
int i;
double a1 = 1 / 3, a2 = 1 / 3, a3 = 1 / 3, b1 = 0.5, b2 = 0.5;
double ap1[3], ap2[3], ap3[3], ap4[3], ap5[3], ap6[3];
double k1=54.0/105.0,k2=92.0/105.0,k3=214.0/105.0;
double temp_q[4],qt[4],qtinv[4];
double q_tmp[4];
double s, beta2, sum, beta[3];
for (i = 0; i < 3; i++) // reason for floating point earth_rateor
{
ap1[i] = 0.0; ap2[i] = 0.0; ap3[i] = 0.0;
ap4[i] = 0.0; ap5[i] = 0.0; ap6[i] = 0.0;
beta[i] = 0.0;
}
for (i = 0; i < 4; i++)
{
temp_q[i] = 0.0;
q_tmp[i] = 0.0;
}
cross(p_alp1,p_alp4,ap1);
cross(p_alp1,p_alp3,ap2);
cross(p_alp2,p_alp3,ap3);
cross(p_alp3,p_alp4,ap4);
cross(p_alp2,p_alp4,ap5);
cross(p_alp1,p_alp2,ap6);
for(i=0;i<3;i++)
{
beta[i] = p_alp1[i]+p_alp2[i]+p_alp3[i]+p_alp4[i]+k1*ap1[i]+k2*(b1*ap2[i]+b2*ap5[i])+k3*(a1*ap6[i]+a2*ap3[i]+a3*ap4[i]);
}
beta2 = (pow(beta[0],2)) + (pow(beta[1],2)) + (pow(beta[2],2));
sum = 0.5 - beta2/48.0;
temp_q[0] = 1.0-beta2/8.0+pow(beta2,2)/480.0;
temp_q[1] = sum*beta[0];
temp_q[2] = sum*beta[1];
temp_q[3] = sum*beta[2];
quat_mult((double *)p_q_body2ecef,(double *)temp_q,(double *)q_tmp);
for(i=0;i< 4;i++)
{
p_q_body2ecef[i] = q_tmp[i];
}
//earth rate corr
for (i = 0; i < 3; i++)
beta[i] = 4 * del_t * omega[i];
beta2 = beta[0] * beta[0] + beta[1] * beta[1] + beta[2] * beta[2];
s = 0.5 - beta2 / 48.0;
qt[0] = 1.0 - beta2 / 8.0 + (beta2 * beta2) / 480.0;
qt[1] = s * beta[0];
qt[2] = s * beta[1];
qt[3] = s * beta[2];
quat_inv(qt,qtinv);
quat_mult(qtinv, p_q_body2ecef, q_tmp);
for (i = 0; i < 4; i++)
p_q_body2ecef[i] = q_tmp[i];
quat_norm((double *)p_q_body2ecef);
} //end of nav_attup
void varinit(void)
{
int i;
/*
* Resetting all flags
*/
Intr1_Cnt=0;
Intr2_Cnt=0;
IRQ1Flag = 1;
IRQ2Flag = 1;
WSZ = 34;
TA_cnt =0;
count = 0;
qcnt = 0;
velcnt = 0;
rtime = 0.0;
rcnt = 0;
cnt_10ms = 0;
latm = MasterLat;
longm = MasterLon;
epsilon = 0.0;
four_delt = 4.0 * del_t;
eight_delt = 8.0 * del_t;
cdr_delt = cdr * del_t;
cdr_delt_ms = cdr_delt / 3600;
for(i=0;i<32;i++){
Array_SA[i] = 0;
}
for(i=0;i<3;i++)
{
velo_ref_y[i] = 0.0;
velo_ref_yold[i] = 0.0;;
velo_ref_x[i] = 0.0;
velo_ref_xold[i] = 0.0;
pure_vel[i] = 0.0;
p_velo_20ms[i] = 0.0;
p_velo[i] = 0.0;
pure_v_old[i] = 0.0;
p_Ang[i] = 0.0;
pure_gyro_drift[i] = 0.0;
pure_acc_residu[i] = 0.0;
}
#if 0
/* these are known misalignment angles between M and S -
* Measured w.r.t Master to give DCM from slave to Master.
* Beware they are not between slave to NED */
known_si = 0.0 * cdr;
known_theta = 0.0 * cdr;
known_phi = 0.0 * cdr;
euler2dcm_stp(0, 0, 0, (double*)CSkew_est);
transpose(3, 3, (double*)CSkew_est, (double*)CSkew_est_T);
euler2dcm_stp(known_si, known_theta, known_phi, (double*)CS2M_K);
transpose(3, 3, (double*)CS2M_K, (double*)CM2S_K);
euler2dcm_stp(THDG, PITCH, ROLL, (double*)Cb2ned_M);
matmul(3, 3, (double*)Cb2ned_M, 3, 3, (double*)CS2M_K, (double*)Cb2ned_S);
if(ta_flag==1 && nav_flag==1)
{
dcm2quat((double*)Cb2ned_S, (double *)p_q_body2ned);
}
else if(ta_flag ==0 && level_flag==1)
#endif
{
euler2quat_spt(mdl_si,mdl_phi,mdl_theta,(double *)p_q_body2ned);
p_si = mdl_si;
p_phi = mdl_phi;
p_theta = mdl_theta;
}
ned2ecef_q(latm, longm,(double*) q_ned2ecef);
quat_mult((double*)q_ned2ecef,(double*)p_q_body2ned, (double*)p_q_body2ecef);
/*
* Modification after Manjit discussion
*/
quat2dcm((double *)p_q_body2ecef,(double*)p_dcm);
quat2dcm((double *)q_ned2ecef,(double*)p_dcm_n);
matmul(3,3, (double*)p_dcm_n,3,1,(double*)MasterVel,(double*)pure_vel);
pure_v_old[0] = pure_vel[0];
pure_v_old[1] = pure_vel[1];
pure_v_old[2] = pure_vel[2];
init(0.0, 0.0, 0.0, p_velo_20ms);
init(0.0, 0.0, 0.0, p_velo);
init(0.0,0.0,0.0,pure_gyro_drift);
init(0.0,0.0,0.0,pure_acc_residu);
for (i = 0; i < 3; i++)
{
p_alp1[i] = 0.0; p_alp2[i] = 0.0; p_alp3[i] = 0.0; p_alp4[i] = 0.0;
}
for (i = 0; i < 3; i++)
Delta_Angle[i] = 0.0;
for (i = 0; i < 6; i++)
accum1[i] = 0.0;
init(0.0, 0.0, earth_rate, omega); //earth rate vector ECEF
//used in levelling
Ned_omega[0] = earth_rate * cos(latm);
Ned_omega[1] = 0.0;
Ned_omega[2] = -earth_rate *sin(latm);
for (i = 0; i < 3; i++)
omg_dub[i] = 2.0 * omega[i];
r_init = r0 * (1.0 - eccen * (sin(latm) * sin(latm)));
pure_R = r_init + MasterAlt; // altitude;
lla2ecef(latm,longm,MasterAlt,(double *)pure_ecef_pos); //input is geodetic
pure_g_ecef();
/**** for epsilon estimation ****/
init(0.0, 0.0, -pure_g_ecef_mag, Ned_gravity_detic);
} //end of varinit()
vec3 quat_transform(quat q, vec3 v) {
quat qv = (quat) { v.x, v.y, v.z, 0.0 };
qv = quat_mult(q, quat_mult(qv, quat_conjugate(q)));
return (vec3) { q.x, q.y, q.z };
}
