// 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 }; }