int EKFINS::get_euler(float *euler) { Quaternion2RPY(&x[0], euler); return 0; }
/** * @brief Quickly initialize INS assuming stationary and gravity is down * * Currently this is done iteratively but I'm sure it can be directly computed * when I sit down and work it out */ void converge_insgps() { AHRSCalibrationData calibration; AHRSCalibrationGet( &calibration ); float pos[3] = { 0, 0, 0 }, vel[3] = { 0, 0, 0 }, BaroAlt = 0, mag[3], accel[3], temp_gyro[3] = { 0, 0, 0 }; INSGPSInit(); INSSetAccelVar( calibration.accel_var ); INSSetGyroBias( temp_gyro ); // set this to zero - crude bias corrected from downsample_data INSSetGyroVar( calibration.gyro_var ); INSSetMagVar( calibration.mag_var ); float temp_var[3] = { 10, 10, 10 }; INSSetGyroVar( temp_var ); // ignore gyro's accel[0] = accel_data.filtered.x; accel[1] = accel_data.filtered.y; accel[2] = accel_data.filtered.z; // Iteratively constrain pitch and roll while updating yaw to align magnetic axis. for( int i = 0; i < 50; i++ ) { // This should be done directly but I'm too dumb. float rpy[3]; Quaternion2RPY( Nav.q, rpy ); rpy[1] = -atan2( accel_data.filtered.x, accel_data.filtered.z ) * 180 / M_PI; rpy[0] = -atan2( accel_data.filtered.y, accel_data.filtered.z ) * 180 / M_PI; // Get magnetic readings #if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C) PIOS_HMC5843_ReadMag( mag_data.raw.axis ); #endif mag[0] = -mag_data.raw.axis[1]; mag[1] = -mag_data.raw.axis[0]; mag[2] = -mag_data.raw.axis[2]; RPY2Quaternion( rpy, Nav.q ); INSStatePrediction( temp_gyro, accel, 1 / ( float )EKF_RATE ); INSCovariancePrediction( 1 / ( float )EKF_RATE ); FullCorrection( mag, pos, vel, BaroAlt ); } INSSetGyroVar( calibration.gyro_var ); }
void updateAttitude(void) { updateSensors(); static uint32_t now, last; float dT; now = micros(); dT = (float)(now - last) * 1e-6f; last = now; AHRSUpdate( stateData.gyro[ROLL], -stateData.gyro[PITCH], stateData.gyro[YAW], stateData.accel[X], stateData.accel[Y], stateData.accel[Z], stateData.mag[X], stateData.mag[Y], stateData.mag[Z], dT); zeroSensorAccumulators(); Quaternion2RPY(stateData.q, &stateData.roll, &stateData.pitch, &stateData.yaw); stateData.heading += cfg.magDeclination * DEG2RAD; }
/** * 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); } }
/** * @brief */ __attribute__((optimize("O3"))) void attitudeUpdate(PIMUStruct pIMU) { float accelErr[3] = {0.0f}; float mag; float dq[4]; pIMU->accelData[0] -= pIMU->accelBias[0]; pIMU->accelData[1] -= pIMU->accelBias[1]; pIMU->accelData[2] -= pIMU->accelBias[2]; // Account for accel's magnitude. mag = QInvSqrtf(pIMU->accelData[0]*pIMU->accelData[0] + pIMU->accelData[1]*pIMU->accelData[1] + pIMU->accelData[2]*pIMU->accelData[2]); if ((mag > 0.0724f) && (mag < 0.1724f)) { float v2[3]; /* Compute estimated direction of gravity halved. * * Rotated gravity vector v2 is calculated by multiplying gravity * vector v={0,0,1} by conjugate (q`) of attitude quaternion (q) (v2=q`vq), * because MPU6050 senses gravity in opposite direction. */ v2[0] = (pIMU->qIMU[1]*pIMU->qIMU[3] - pIMU->qIMU[0]*pIMU->qIMU[2]); v2[1] = (pIMU->qIMU[2]*pIMU->qIMU[3] + pIMU->qIMU[0]*pIMU->qIMU[1]); v2[2] = (pIMU->qIMU[0]*pIMU->qIMU[0] + pIMU->qIMU[3]*pIMU->qIMU[3]) - 0.5f; // Apply smoothing to accel values, to reduce vibration noise before main calculations. accelFilterApply(pIMU->accelData, pIMU->accelFiltered); // Apply the same filtering to the estimated direction of gravity to match phase shift. accelFilterApply(v2, pIMU->v2Filtered); // Compute the error between the predicted direction of gravity and smoothed acceleration. CrossProduct(pIMU->accelFiltered, pIMU->v2Filtered, accelErr); // Normalize accel_error. accelErr[0] *= mag; accelErr[1] *= mag; accelErr[2] *= mag; } // Correct rates based on error. pIMU->gyroBias[0] -= accelErr[0]*accel2Ki; pIMU->gyroBias[1] -= accelErr[1]*accel2Ki; pIMU->gyroBias[2] -= accelErr[2]*accel2Ki; pIMU->gyroData[0] -= pIMU->gyroBias[0]; pIMU->gyroData[1] -= pIMU->gyroBias[1]; pIMU->gyroData[2] -= pIMU->gyroBias[2]; // Correct rates based on error. pIMU->gyroData[0] += accelErr[0]*accel2Kp; pIMU->gyroData[1] += accelErr[1]*accel2Kp; pIMU->gyroData[2] += accelErr[2]*accel2Kp; dq[0] = (-pIMU->qIMU[1]*pIMU->gyroData[0] - pIMU->qIMU[2]*pIMU->gyroData[1] - pIMU->qIMU[3]*pIMU->gyroData[2])*(FIXED_DT_STEP*DEG2RAD*0.5f); dq[1] = ( pIMU->qIMU[0]*pIMU->gyroData[0] - pIMU->qIMU[3]*pIMU->gyroData[1] + pIMU->qIMU[2]*pIMU->gyroData[2])*(FIXED_DT_STEP*DEG2RAD*0.5f); dq[2] = ( pIMU->qIMU[3]*pIMU->gyroData[0] + pIMU->qIMU[0]*pIMU->gyroData[1] - pIMU->qIMU[1]*pIMU->gyroData[2])*(FIXED_DT_STEP*DEG2RAD*0.5f); dq[3] = (-pIMU->qIMU[2]*pIMU->gyroData[0] + pIMU->qIMU[1]*pIMU->gyroData[1] + pIMU->qIMU[0]*pIMU->gyroData[2])*(FIXED_DT_STEP*DEG2RAD*0.5f); pIMU->qIMU[0] += dq[0]; pIMU->qIMU[1] += dq[1]; pIMU->qIMU[2] += dq[2]; pIMU->qIMU[3] += dq[3]; // Normalize attitude quaternion. mag = QInvSqrtf(pIMU->qIMU[0]*pIMU->qIMU[0] + pIMU->qIMU[1]*pIMU->qIMU[1] + pIMU->qIMU[2]*pIMU->qIMU[2] + pIMU->qIMU[3]*pIMU->qIMU[3]); pIMU->qIMU[0] *= mag; pIMU->qIMU[1] *= mag; pIMU->qIMU[2] *= mag; pIMU->qIMU[3] *= mag; // Convert attitude into Euler angles; Quaternion2RPY(pIMU->qIMU, pIMU->rpy); }
static void updateAttitude(AttitudeRawData * attitudeRaw) { AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); static portTickType lastSysTime = 0; static portTickType thisSysTime; static float dT = 0; thisSysTime = xTaskGetTickCount(); if(thisSysTime > lastSysTime) // reuse dt in case of wraparound dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; lastSysTime = thisSysTime; // Bad practice to assume structure order, but saves memory float * q = &attitudeActual.q1; float gyro[3]; gyro[0] = attitudeRaw->gyros[0]; gyro[1] = attitudeRaw->gyros[1]; gyro[2] = attitudeRaw->gyros[2]; { float * accels = attitudeRaw->accels; float grot[3]; float accel_err[3]; // Rotate gravity to body frame and cross with accels grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); CrossProduct((const float *) accels, (const float *) grot, accel_err); // Account for accel magnitude float accel_mag = sqrt(accels[0]*accels[0] + accels[1]*accels[1] + accels[2]*accels[2]); accel_err[0] /= accel_mag; accel_err[1] /= accel_mag; accel_err[2] /= accel_mag; // Accumulate integral of error. Scale here so that units are (rad/s) but Ki has units of s gyro_correct_int[0] += accel_err[0] * accelKi; gyro_correct_int[1] += accel_err[1] * accelKi; //gyro_correct_int[2] += accel_err[2] * settings.AccelKI * dT; // Correct rates based on error, integral component dealt with in updateSensors gyro[0] += accel_err[0] * accelKp / dT; gyro[1] += accel_err[1] * accelKp / dT; gyro[2] += accel_err[2] * accelKp / dT; } { // scoping variables to save memory // Work out time derivative from INSAlgo writeup // Also accounts for the fact that gyros are in deg/s float qdot[4]; qdot[0] = (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]) * dT * M_PI / 180 / 2; qdot[1] = (q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * M_PI / 180 / 2; qdot[2] = (q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * M_PI / 180 / 2; qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2; // Take a time step q[0] = q[0] + qdot[0]; q[1] = q[1] + qdot[1]; q[2] = q[2] + qdot[2]; q[3] = q[3] + qdot[3]; } // Renomalize float qmag = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); q[0] = q[0] / qmag; q[1] = q[1] / qmag; q[2] = q[2] / qmag; q[3] = q[3] / qmag; attitudeActual.q1 = q[0]; attitudeActual.q2 = q[1]; attitudeActual.q3 = q[2]; attitudeActual.q4 = q[3]; // Convert into eueler degrees (makes assumptions about RPY order) Quaternion2RPY(q,&attitudeActual.Roll); AttitudeActualSet(&attitudeActual); }
static void updateAttitude(AttitudeRawData * attitudeRaw) { float dT; portTickType thisSysTime = xTaskGetTickCount(); static portTickType lastSysTime = 0; dT = (thisSysTime == lastSysTime) ? 0.001 : (portMAX_DELAY & (thisSysTime - lastSysTime)) / portTICK_RATE_MS / 1000.0f; lastSysTime = thisSysTime; // Bad practice to assume structure order, but saves memory float gyro[3]; gyro[0] = attitudeRaw->gyros[0]; gyro[1] = attitudeRaw->gyros[1]; gyro[2] = attitudeRaw->gyros[2]; { float * accels = attitudeRaw->accels; float grot[3]; float accel_err[3]; // Rotate gravity to body frame and cross with accels grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); CrossProduct((const float *) accels, (const float *) grot, accel_err); // Account for accel magnitude float accel_mag = sqrt(accels[0]*accels[0] + accels[1]*accels[1] + accels[2]*accels[2]); accel_err[0] /= accel_mag; accel_err[1] /= accel_mag; accel_err[2] /= accel_mag; // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s gyro_correct_int[0] += accel_err[0] * accelKi; gyro_correct_int[1] += accel_err[1] * accelKi; //gyro_correct_int[2] += accel_err[2] * settings.AccelKI * dT; // Correct rates based on error, integral component dealt with in updateSensors gyro[0] += accel_err[0] * accelKp / dT; gyro[1] += accel_err[1] * accelKp / dT; gyro[2] += accel_err[2] * accelKp / dT; } { // scoping variables to save memory // Work out time derivative from INSAlgo writeup // Also accounts for the fact that gyros are in deg/s float qdot[4]; qdot[0] = (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]) * dT * M_PI / 180 / 2; qdot[1] = (q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * M_PI / 180 / 2; qdot[2] = (q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * M_PI / 180 / 2; qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2; // Take a time step q[0] = q[0] + qdot[0]; q[1] = q[1] + qdot[1]; q[2] = q[2] + qdot[2]; q[3] = q[3] + qdot[3]; } // Renomalize float qmag = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); q[0] = q[0] / qmag; q[1] = q[1] / qmag; q[2] = q[2] / qmag; q[3] = q[3] / qmag; // If quaternion has become inappropriately short or is nan reinit. // THIS SHOULD NEVER ACTUALLY HAPPEN if((fabs(qmag) < 1e-3) || (qmag != qmag)) { q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; } AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); quat_copy(q, &attitudeActual.q1); // Convert into eueler degrees (makes assumptions about RPY order) Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll); AttitudeActualSet(&attitudeActual); }
/** * This method performs a simple simulation of a car * * It takes in the ActuatorDesired command to rotate the aircraft and performs * a simple kinetic model where the throttle increases the energy and drag decreases * it. Changing altitude moves energy from kinetic to potential. * * 1. Update attitude based on ActuatorDesired * 2. Update position based on velocity */ static void simulateModelCar() { static double pos[3] = {0,0,0}; static double vel[3] = {0,0,0}; static double ned_accel[3] = {0,0,0}; static float q[4] = {1,0,0,0}; static float rpy[3] = {0,0,0}; // Low pass filtered actuator static float baro_offset = 0.0f; float Rbe[3][3]; const float ACTUATOR_ALPHA = 0.8; const float MAX_THRUST = 9.81 * 0.5; const float K_FRICTION = 0.2; const float GPS_PERIOD = 0.1; const float MAG_PERIOD = 1.0 / 75.0; const float BARO_PERIOD = 1.0 / 20.0; static uint32_t last_time; float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6); if(dT < 1e-3) dT = 2e-3; last_time = PIOS_DELAY_GetRaw(); FlightStatusData flightStatus; FlightStatusGet(&flightStatus); ActuatorDesiredData actuatorDesired; ActuatorDesiredGet(&actuatorDesired); float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0; if (thrust < 0) thrust = 0; if (thrust != thrust) thrust = 0; // float control_scaling = thrust * thrustToDegs; // // In rad/s // rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; // rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; // rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; // // GyrosData gyrosData; // Skip get as we set all the fields // gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss(); // gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss(); // gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss(); /**** 1. Update attitude ****/ RateDesiredData rateDesired; RateDesiredGet(&rateDesired); // Need to get roll angle for easy cross coupling AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); rpy[0] = 0; // cannot roll rpy[1] = 0; // cannot pitch rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; GyrosData gyrosData; // Skip get as we set all the fields gyrosData.x = rpy[0] + rand_gauss(); gyrosData.y = rpy[1] + rand_gauss(); gyrosData.z = rpy[2] + rand_gauss(); GyrosSet(&gyrosData); // Predict the attitude forward in time float qdot[4]; qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * DEG2RAD / 2; qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * DEG2RAD / 2; qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * DEG2RAD / 2; qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * DEG2RAD / 2; // Take a time step q[0] = q[0] + qdot[0]; q[1] = q[1] + qdot[1]; q[2] = q[2] + qdot[2]; q[3] = q[3] + qdot[3]; float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); q[0] = q[0] / qmag; q[1] = q[1] / qmag; q[2] = q[2] / qmag; q[3] = q[3] / qmag; if(overideAttitude){ AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); attitudeActual.q1 = q[0]; attitudeActual.q2 = q[1]; attitudeActual.q3 = q[2]; attitudeActual.q4 = q[3]; AttitudeActualSet(&attitudeActual); } /**** 2. Update position based on velocity ****/ // Rbe takes a vector from body to earth. If we take (1,0,0)^T through this and then dot with airspeed // we get forward airspeed Quaternion2R(q,Rbe); double groundspeed[3] = {vel[0], vel[1], vel[2] }; double forwardSpeed = Rbe[0][0] * groundspeed[0] + Rbe[0][1] * groundspeed[1] + Rbe[0][2] * groundspeed[2]; double sidewaysSpeed = Rbe[1][0] * groundspeed[0] + Rbe[1][1] * groundspeed[1] + Rbe[1][2] * groundspeed[2]; /* Compute aerodynamic forces in body referenced frame. Later use more sophisticated equations */ /* TODO: This should become more accurate. Use the force equations to calculate lift from the */ /* various surfaces based on AoA and airspeed. From that compute torques and forces. For later */ double forces[3]; // X, Y, Z forces[0] = thrust - forwardSpeed * K_FRICTION; // Friction is applied in all directions in NED forces[1] = 0 - sidewaysSpeed * K_FRICTION * 100; // No side slip forces[2] = 0; // Negate force[2] as NED defines down as possitive, aircraft convention is Z up is positive (?) ned_accel[0] = forces[0] * Rbe[0][0] + forces[1] * Rbe[1][0] - forces[2] * Rbe[2][0]; ned_accel[1] = forces[0] * Rbe[0][1] + forces[1] * Rbe[1][1] - forces[2] * Rbe[2][1]; ned_accel[2] = 0; // Apply acceleration based on velocity ned_accel[0] -= K_FRICTION * (vel[0]); ned_accel[1] -= K_FRICTION * (vel[1]); // Predict the velocity forward in time vel[0] = vel[0] + ned_accel[0] * dT; vel[1] = vel[1] + ned_accel[1] * dT; vel[2] = vel[2] + ned_accel[2] * dT; // Predict the position forward in time pos[0] = pos[0] + vel[0] * dT; pos[1] = pos[1] + vel[1] * dT; pos[2] = pos[2] + vel[2] * dT; // Simulate hitting ground if(pos[2] > 0) { pos[2] = 0; vel[2] = 0; ned_accel[2] = 0; } // Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0) ned_accel[2] -= GRAVITY; // Transform the accels back in to body frame AccelsData accelsData; // Skip get as we set all the fields accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0]; accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1]; accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2]; accelsData.temperature = 30; AccelsSet(&accelsData); if(baro_offset == 0) { // Hacky initialization baro_offset = 50;// * rand_gauss(); } else { // Very small drift process baro_offset += rand_gauss() / 100; } // Update baro periodically static uint32_t last_baro_time = 0; if(PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) { BaroAltitudeData baroAltitude; BaroAltitudeGet(&baroAltitude); baroAltitude.Altitude = -pos[2] + baro_offset; BaroAltitudeSet(&baroAltitude); last_baro_time = PIOS_DELAY_GetRaw(); } HomeLocationData homeLocation; HomeLocationGet(&homeLocation); static float gps_vel_drift[3] = {0,0,0}; gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0; gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0; gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0; // Update GPS periodically static uint32_t last_gps_time = 0; if(PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) { // Use double precision here as simulating what GPS produces double T[3]; T[0] = homeLocation.Altitude+6.378137E6f * DEG2RAD; T[1] = cosf(homeLocation.Latitude / 10e6 * DEG2RAD)*(homeLocation.Altitude+6.378137E6) * DEG2RAD; T[2] = -1.0; static float gps_drift[3] = {0,0,0}; gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0; gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0; gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0; GPSPositionData gpsPosition; GPSPositionGet(&gpsPosition); gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6); gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6); gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]); gpsPosition.Groundspeed = sqrtf(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2)); gpsPosition.Heading = 180 / M_PI * atan2f(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]); gpsPosition.Satellites = 7; gpsPosition.PDOP = 1; GPSPositionSet(&gpsPosition); last_gps_time = PIOS_DELAY_GetRaw(); } // Update GPS Velocity measurements static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond if(PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) { GPSVelocityData gpsVelocity; GPSVelocityGet(&gpsVelocity); gpsVelocity.North = vel[0] + gps_vel_drift[0]; gpsVelocity.East = vel[1] + gps_vel_drift[1]; gpsVelocity.Down = vel[2] + gps_vel_drift[2]; GPSVelocitySet(&gpsVelocity); last_gps_vel_time = PIOS_DELAY_GetRaw(); } // Update mag periodically static uint32_t last_mag_time = 0; if(PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) { MagnetometerData mag; mag.x = 100+homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; mag.y = 100+homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2]; mag.z = 100+homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2]; magOffsetEstimation(&mag); MagnetometerSet(&mag); last_mag_time = PIOS_DELAY_GetRaw(); } AttitudeSimulatedData attitudeSimulated; AttitudeSimulatedGet(&attitudeSimulated); attitudeSimulated.q1 = q[0]; attitudeSimulated.q2 = q[1]; attitudeSimulated.q3 = q[2]; attitudeSimulated.q4 = q[3]; Quaternion2RPY(q,&attitudeSimulated.Roll); attitudeSimulated.Position[0] = pos[0]; attitudeSimulated.Position[1] = pos[1]; attitudeSimulated.Position[2] = pos[2]; attitudeSimulated.Velocity[0] = vel[0]; attitudeSimulated.Velocity[1] = vel[1]; attitudeSimulated.Velocity[2] = vel[2]; AttitudeSimulatedSet(&attitudeSimulated); }
static void simulateModelQuadcopter() { static double pos[3] = {0,0,0}; static double vel[3] = {0,0,0}; static double ned_accel[3] = {0,0,0}; static float q[4] = {1,0,0,0}; static float rpy[3] = {0,0,0}; // Low pass filtered actuator static float baro_offset = 0.0f; static float temperature = 20; float Rbe[3][3]; const float ACTUATOR_ALPHA = 0.8; const float MAX_THRUST = GRAVITY * 2; const float K_FRICTION = 1; const float GPS_PERIOD = 0.1; const float MAG_PERIOD = 1.0 / 75.0; const float BARO_PERIOD = 1.0 / 20.0; static uint32_t last_time; float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6); if(dT < 1e-3) dT = 2e-3; last_time = PIOS_DELAY_GetRaw(); FlightStatusData flightStatus; FlightStatusGet(&flightStatus); ActuatorDesiredData actuatorDesired; ActuatorDesiredGet(&actuatorDesired); float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0; if (thrust < 0) thrust = 0; if (thrust != thrust) thrust = 0; // float control_scaling = thrust * thrustToDegs; // // In rad/s // rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; // rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; // rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; // // GyrosData gyrosData; // Skip get as we set all the fields // gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss(); // gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss(); // gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss(); RateDesiredData rateDesired; RateDesiredGet(&rateDesired); rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; temperature = 20; GyrosData gyrosData; // Skip get as we set all the fields gyrosData.x = rpy[0] + rand_gauss() + (temperature - 20) * 1 + powf(temperature - 20,2) * 0.11; // - powf(temperature - 20,3) * 0.05;; gyrosData.y = rpy[1] + rand_gauss() + (temperature - 20) * 1 + powf(temperature - 20,2) * 0.11;; gyrosData.z = rpy[2] + rand_gauss() + (temperature - 20) * 1 + powf(temperature - 20,2) * 0.11;; gyrosData.temperature = temperature; GyrosSet(&gyrosData); // Predict the attitude forward in time float qdot[4]; qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * DEG2RAD / 2; qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * DEG2RAD / 2; qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * DEG2RAD / 2; qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * DEG2RAD / 2; // Take a time step q[0] = q[0] + qdot[0]; q[1] = q[1] + qdot[1]; q[2] = q[2] + qdot[2]; q[3] = q[3] + qdot[3]; float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); q[0] = q[0] / qmag; q[1] = q[1] / qmag; q[2] = q[2] / qmag; q[3] = q[3] / qmag; if(overideAttitude){ AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); attitudeActual.q1 = q[0]; attitudeActual.q2 = q[1]; attitudeActual.q3 = q[2]; attitudeActual.q4 = q[3]; AttitudeActualSet(&attitudeActual); } static float wind[3] = {0,0,0}; wind[0] = wind[0] * 0.95 + rand_gauss() / 10.0; wind[1] = wind[1] * 0.95 + rand_gauss() / 10.0; wind[2] = wind[2] * 0.95 + rand_gauss() / 10.0; Quaternion2R(q,Rbe); // Make thrust negative as down is positive ned_accel[0] = -thrust * Rbe[2][0]; ned_accel[1] = -thrust * Rbe[2][1]; // Gravity causes acceleration of 9.81 in the down direction ned_accel[2] = -thrust * Rbe[2][2] + GRAVITY; // Apply acceleration based on velocity ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]); ned_accel[1] -= K_FRICTION * (vel[1] - wind[1]); ned_accel[2] -= K_FRICTION * (vel[2] - wind[2]); // Predict the velocity forward in time vel[0] = vel[0] + ned_accel[0] * dT; vel[1] = vel[1] + ned_accel[1] * dT; vel[2] = vel[2] + ned_accel[2] * dT; // Predict the position forward in time pos[0] = pos[0] + vel[0] * dT; pos[1] = pos[1] + vel[1] * dT; pos[2] = pos[2] + vel[2] * dT; // Simulate hitting ground if(pos[2] > 0) { pos[2] = 0; vel[2] = 0; ned_accel[2] = 0; } // Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0) ned_accel[2] -= 9.81; // Transform the accels back in to body frame AccelsData accelsData; // Skip get as we set all the fields accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0]; accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1]; accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2]; accelsData.temperature = 30; AccelsSet(&accelsData); if(baro_offset == 0) { // Hacky initialization baro_offset = 50;// * rand_gauss(); } else { // Very small drift process baro_offset += rand_gauss() / 100; } // Update baro periodically static uint32_t last_baro_time = 0; if(PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) { BaroAltitudeData baroAltitude; BaroAltitudeGet(&baroAltitude); baroAltitude.Altitude = -pos[2] + baro_offset; BaroAltitudeSet(&baroAltitude); last_baro_time = PIOS_DELAY_GetRaw(); } HomeLocationData homeLocation; HomeLocationGet(&homeLocation); static float gps_vel_drift[3] = {0,0,0}; gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0; gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0; gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0; // Update GPS periodically static uint32_t last_gps_time = 0; if(PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) { // Use double precision here as simulating what GPS produces double T[3]; T[0] = homeLocation.Altitude+6.378137E6f * DEG2RAD; T[1] = cosf(homeLocation.Latitude / 10e6 * DEG2RAD)*(homeLocation.Altitude+6.378137E6) * DEG2RAD; T[2] = -1.0; static float gps_drift[3] = {0,0,0}; gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0; gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0; gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0; GPSPositionData gpsPosition; GPSPositionGet(&gpsPosition); gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6); gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6); gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]); gpsPosition.Groundspeed = sqrtf(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2)); gpsPosition.Heading = 180 / M_PI * atan2f(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]); gpsPosition.Satellites = 7; gpsPosition.PDOP = 1; gpsPosition.Status = GPSPOSITION_STATUS_FIX3D; GPSPositionSet(&gpsPosition); last_gps_time = PIOS_DELAY_GetRaw(); } // Update GPS Velocity measurements static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond if(PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) { GPSVelocityData gpsVelocity; GPSVelocityGet(&gpsVelocity); gpsVelocity.North = vel[0] + gps_vel_drift[0]; gpsVelocity.East = vel[1] + gps_vel_drift[1]; gpsVelocity.Down = vel[2] + gps_vel_drift[2]; GPSVelocitySet(&gpsVelocity); last_gps_vel_time = PIOS_DELAY_GetRaw(); } // Update mag periodically static uint32_t last_mag_time = 0; if(PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) { MagnetometerData mag; mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2]; mag.y = homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2]; mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2]; // Run the offset compensation algorithm from the firmware magOffsetEstimation(&mag); MagnetometerSet(&mag); last_mag_time = PIOS_DELAY_GetRaw(); } AttitudeSimulatedData attitudeSimulated; AttitudeSimulatedGet(&attitudeSimulated); attitudeSimulated.q1 = q[0]; attitudeSimulated.q2 = q[1]; attitudeSimulated.q3 = q[2]; attitudeSimulated.q4 = q[3]; Quaternion2RPY(q,&attitudeSimulated.Roll); attitudeSimulated.Position[0] = pos[0]; attitudeSimulated.Position[1] = pos[1]; attitudeSimulated.Position[2] = pos[2]; attitudeSimulated.Velocity[0] = vel[0]; attitudeSimulated.Velocity[1] = vel[1]; attitudeSimulated.Velocity[2] = vel[2]; AttitudeSimulatedSet(&attitudeSimulated); }
/** * @brief Use the INSGPS fusion algorithm in either indoor or outdoor mode (use GPS) * @params[in] first_run This is the first run so trigger reinitialization * @params[in] outdoor_mode If true use the GPS for position, if false weakly pull to (0,0) * @return 0 for success, -1 for failure */ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) { UAVObjEvent ev; GyrosData gyrosData; AccelsData accelsData; MagnetometerData magData; BaroAltitudeData baroData; GPSPositionData gpsData; GPSVelocityData gpsVelData; GyrosBiasData gyrosBias; static bool mag_updated = false; static bool baro_updated; static bool gps_updated; static bool gps_vel_updated; static float baroOffset = 0; static uint32_t ins_last_time = 0; static bool inited; float NED[3] = {0.0f, 0.0f, 0.0f}; float vel[3] = {0.0f, 0.0f, 0.0f}; float zeros[3] = {0.0f, 0.0f, 0.0f}; // Perform the update uint16_t sensors = 0; float dT; // Wait until the gyro and accel object is updated, if a timeout then go to failsafe if ( (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) || (xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) ) { // Do not set attitude timeout warnings in simulation mode if (!AttitudeActualReadOnly()){ AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING); return -1; } } if (inited) { mag_updated = 0; baro_updated = 0; gps_updated = 0; gps_vel_updated = 0; } if (first_run) { inited = false; init_stage = 0; mag_updated = 0; baro_updated = 0; gps_updated = 0; gps_vel_updated = 0; ins_last_time = PIOS_DELAY_GetRaw(); return 0; } mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE); baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; // Get most recent data GyrosGet(&gyrosData); AccelsGet(&accelsData); MagnetometerGet(&magData); BaroAltitudeGet(&baroData); GPSPositionGet(&gpsData); GPSVelocityGet(&gpsVelData); GyrosBiasGet(&gyrosBias); // Discard mag if it has NAN (normally from bad calibration) mag_updated &= (magData.x == magData.x && magData.y == magData.y && magData.z == magData.z); // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily // switching between indoor and outdoor mode with Set = false) mag_updated &= (homeLocation.Be[0] != 0 || homeLocation.Be[1] != 0 || homeLocation.Be[2]); // Have a minimum requirement for gps usage gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP <= 4.0f) && (homeLocation.Set == HOMELOCATION_SET_TRUE); if (!inited) AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); else if (outdoor_mode && gpsData.Satellites < 7) AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); else AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode)) { // Don't initialize until all sensors are read if (init_stage == 0 && !outdoor_mode) { float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f}; float q[4]; float pos[3] = {0.0f, 0.0f, 0.0f}; // Initialize barometric offset to homelocation altitude baroOffset = -baroData.Altitude; pos[2] = -(baroData.Altitude + baroOffset); // Reset the INS algorithm INSGPSInit(); INSSetMagVar(revoCalibration.mag_var); INSSetAccelVar(revoCalibration.accel_var); INSSetGyroVar(revoCalibration.gyro_var); INSSetBaroVar(revoCalibration.baro_var); // Initialize the gyro bias from the settings float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f}; INSSetGyroBias(gyro_bias); xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); MagnetometerGet(&magData); // Set initial attitude AttitudeActualData attitudeActual; attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI; attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI; RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); AttitudeActualSet(&attitudeActual); q[0] = attitudeActual.q1; q[1] = attitudeActual.q2; q[2] = attitudeActual.q3; q[3] = attitudeActual.q4; INSSetState(pos, zeros, q, zeros, zeros); INSResetP(Pdiag); } else if (init_stage == 0 && outdoor_mode) { float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f}; float q[4]; float NED[3]; // Reset the INS algorithm INSGPSInit(); INSSetMagVar(revoCalibration.mag_var); INSSetAccelVar(revoCalibration.accel_var); INSSetGyroVar(revoCalibration.gyro_var); INSSetBaroVar(revoCalibration.baro_var); INSSetMagNorth(homeLocation.Be); // Initialize the gyro bias from the settings float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f}; INSSetGyroBias(gyro_bias); GPSPositionData gpsPosition; GPSPositionGet(&gpsPosition); // Transform the GPS position into NED coordinates getNED(&gpsPosition, NED); // Initialize barometric offset to cirrent GPS NED coordinate baroOffset = -NED[2] - baroData.Altitude; xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); MagnetometerGet(&magData); // Set initial attitude AttitudeActualData attitudeActual; attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI; attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI; RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); AttitudeActualSet(&attitudeActual); q[0] = attitudeActual.q1; q[1] = attitudeActual.q2; q[2] = attitudeActual.q3; q[3] = attitudeActual.q4; INSSetState(NED, zeros, q, zeros, zeros); INSResetP(Pdiag); } else if (init_stage > 0) { // Run prediction a bit before any corrections dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; GyrosBiasGet(&gyrosBias); float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f, (gyrosData.y + gyrosBias.y) * F_PI / 180.0f, (gyrosData.z + gyrosBias.z) * F_PI / 180.0f}; INSStatePrediction(gyros, &accelsData.x, dT); AttitudeActualData attitude; AttitudeActualGet(&attitude); attitude.q1 = Nav.q[0]; attitude.q2 = Nav.q[1]; attitude.q3 = Nav.q[2]; attitude.q4 = Nav.q[3]; Quaternion2RPY(&attitude.q1,&attitude.Roll); AttitudeActualSet(&attitude); } init_stage++; if(init_stage > 10) inited = true; ins_last_time = PIOS_DELAY_GetRaw(); return 0; } if (!inited) return 0; dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; ins_last_time = PIOS_DELAY_GetRaw(); // This should only happen at start up or at mode switches if(dT > 0.01f) dT = 0.01f; else if(dT <= 0.001f) dT = 0.001f; // If the gyro bias setting was updated we should reset // the state estimate of the EKF if(gyroBiasSettingsUpdated) { float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f}; INSSetGyroBias(gyro_bias); gyroBiasSettingsUpdated = false; } // Because the sensor module remove the bias we need to add it // back in here so that the INS algorithm can track it correctly float gyros[3] = {gyrosData.x * F_PI / 180.0f, gyrosData.y * F_PI / 180.0f, gyrosData.z * F_PI / 180.0f}; if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { gyros[0] += gyrosBias.x * F_PI / 180.0f; gyros[1] += gyrosBias.y * F_PI / 180.0f; gyros[2] += gyrosBias.z * F_PI / 180.0f; } // Advance the state estimate INSStatePrediction(gyros, &accelsData.x, dT); // Copy the attitude into the UAVO AttitudeActualData attitude; AttitudeActualGet(&attitude); attitude.q1 = Nav.q[0]; attitude.q2 = Nav.q[1]; attitude.q3 = Nav.q[2]; attitude.q4 = Nav.q[3]; Quaternion2RPY(&attitude.q1,&attitude.Roll); AttitudeActualSet(&attitude); // Advance the covariance estimate INSCovariancePrediction(dT); if(mag_updated) sensors |= MAG_SENSORS; if(baro_updated) sensors |= BARO_SENSOR; INSSetMagNorth(homeLocation.Be); if (gps_updated && outdoor_mode) { INSSetPosVelVar(revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_POS], revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_VEL]); sensors |= POS_SENSORS; if (0) { // Old code to take horizontal velocity from GPS Position update sensors |= HORIZ_SENSORS; vel[0] = gpsData.Groundspeed * cosf(gpsData.Heading * F_PI / 180.0f); vel[1] = gpsData.Groundspeed * sinf(gpsData.Heading * F_PI / 180.0f); vel[2] = 0; } // Transform the GPS position into NED coordinates getNED(&gpsData, NED); // Track barometric altitude offset with a low pass filter baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + (1.0f - BARO_OFFSET_LOWPASS_ALPHA ) * ( -NED[2] - baroData.Altitude ); } else if (!outdoor_mode) { INSSetPosVelVar(1e2f, 1e2f); vel[0] = vel[1] = vel[2] = 0; NED[0] = NED[1] = 0; NED[2] = -(baroData.Altitude + baroOffset); sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS; sensors |= POS_SENSORS |VERT_SENSORS; } if (gps_vel_updated && outdoor_mode) { sensors |= HORIZ_SENSORS | VERT_SENSORS; vel[0] = gpsVelData.North; vel[1] = gpsVelData.East; vel[2] = gpsVelData.Down; } /* * TODO: Need to add a general sanity check for all the inputs to make sure their kosher * although probably should occur within INS itself */ if (sensors) INSCorrection(&magData.x, NED, vel, ( baroData.Altitude + baroOffset ), sensors); // Copy the position and velocity into the UAVO PositionActualData positionActual; PositionActualGet(&positionActual); positionActual.North = Nav.Pos[0]; positionActual.East = Nav.Pos[1]; positionActual.Down = Nav.Pos[2]; PositionActualSet(&positionActual); VelocityActualData velocityActual; VelocityActualGet(&velocityActual); velocityActual.North = Nav.Vel[0]; velocityActual.East = Nav.Vel[1]; velocityActual.Down = Nav.Vel[2]; VelocityActualSet(&velocityActual); if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE && !gyroBiasSettingsUpdated) { // Copy the gyro bias into the UAVO except when it was updated // from the settings during the calculation, then consume it // next cycle gyrosBias.x = Nav.gyro_bias[0] * 180.0f / F_PI; gyrosBias.y = Nav.gyro_bias[1] * 180.0f / F_PI; gyrosBias.z = Nav.gyro_bias[2] * 180.0f / F_PI; GyrosBiasSet(&gyrosBias); } return 0; }
static int32_t updateAttitudeComplementary(bool first_run) { UAVObjEvent ev; GyrosData gyrosData; AccelsData accelsData; static int32_t timeval; float dT; static uint8_t init = 0; // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if ( xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE || xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE ) { // When one of these is updated so should the other // Do not set attitude timeout warnings in simulation mode if (!AttitudeActualReadOnly()){ AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING); return -1; } } AccelsGet(&accelsData); // During initialization and FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if(first_run) { #if defined(PIOS_INCLUDE_HMC5883) // To initialize we need a valid mag reading if ( xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE ) return -1; MagnetometerData magData; MagnetometerGet(&magData); #else MagnetometerData magData; magData.x = 100; magData.y = 0; magData.z = 0; #endif AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); init = 0; attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI; attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI; RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); AttitudeActualSet(&attitudeActual); timeval = PIOS_DELAY_GetRaw(); return 0; } if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { // For first 7 seconds use accels to get gyro bias attitudeSettings.AccelKp = 1; attitudeSettings.AccelKi = 0.9; attitudeSettings.YawBiasRate = 0.23; magKp = 1; } else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { attitudeSettings.AccelKp = 1; attitudeSettings.AccelKi = 0.9; attitudeSettings.YawBiasRate = 0.23; magKp = 1; init = 0; } else if (init == 0) { // Reload settings (all the rates) AttitudeSettingsGet(&attitudeSettings); magKp = 0.01f; init = 1; } GyrosGet(&gyrosData); // Compute the dT using the cpu clock dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; timeval = PIOS_DELAY_GetRaw(); float q[4]; AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); float grot[3]; float accel_err[3]; // Get the current attitude estimate quat_copy(&attitudeActual.q1, q); // Rotate gravity to body frame and cross with accels grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err); // Account for accel magnitude accel_mag = accelsData.x*accelsData.x + accelsData.y*accelsData.y + accelsData.z*accelsData.z; accel_mag = sqrtf(accel_mag); accel_err[0] /= accel_mag; accel_err[1] /= accel_mag; accel_err[2] /= accel_mag; if ( xQueueReceive(magQueue, &ev, 0) != pdTRUE ) { // Rotate gravity to body frame and cross with accels float brot[3]; float Rbe[3][3]; MagnetometerData mag; Quaternion2R(q, Rbe); MagnetometerGet(&mag); // If the mag is producing bad data don't use it (normally bad calibration) if (mag.x == mag.x && mag.y == mag.y && mag.z == mag.z) { rot_mult(Rbe, homeLocation.Be, brot); float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z); mag.x /= mag_len; mag.y /= mag_len; mag.z /= mag_len; float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]); brot[0] /= bmag; brot[1] /= bmag; brot[2] /= bmag; // Only compute if neither vector is null if (bmag < 1 || mag_len < 1) mag_err[0] = mag_err[1] = mag_err[2] = 0; else CrossProduct((const float *) &mag.x, (const float *) brot, mag_err); } } else { mag_err[0] = mag_err[1] = mag_err[2] = 0; } // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s GyrosBiasData gyrosBias; GyrosBiasGet(&gyrosBias); gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi; gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi; gyrosBias.z -= mag_err[2] * magKi; GyrosBiasSet(&gyrosBias); // Correct rates based on error, integral component dealt with in updateSensors gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT; gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT; gyrosData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT; // Work out time derivative from INSAlgo writeup // Also accounts for the fact that gyros are in deg/s float qdot[4]; qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * F_PI / 180 / 2; qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * F_PI / 180 / 2; qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * F_PI / 180 / 2; qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * F_PI / 180 / 2; // Take a time step q[0] = q[0] + qdot[0]; q[1] = q[1] + qdot[1]; q[2] = q[2] + qdot[2]; q[3] = q[3] + qdot[3]; if(q[0] < 0) { q[0] = -q[0]; q[1] = -q[1]; q[2] = -q[2]; q[3] = -q[3]; } // Renomalize qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); q[0] = q[0] / qmag; q[1] = q[1] / qmag; q[2] = q[2] / qmag; q[3] = q[3] / qmag; // If quaternion has become inappropriately short or is nan reinit. // THIS SHOULD NEVER ACTUALLY HAPPEN if((fabs(qmag) < 1.0e-3f) || (qmag != qmag)) { q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; } quat_copy(q, &attitudeActual.q1); // Convert into eueler degrees (makes assumptions about RPY order) Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll); AttitudeActualSet(&attitudeActual); // Flush these queues for avoid errors xQueueReceive(baroQueue, &ev, 0); if ( xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE ) { float NED[3]; // Transform the GPS position into NED coordinates GPSPositionData gpsPosition; GPSPositionGet(&gpsPosition); getNED(&gpsPosition, NED); PositionActualData positionActual; PositionActualGet(&positionActual); positionActual.North = NED[0]; positionActual.East = NED[1]; positionActual.Down = NED[2]; PositionActualSet(&positionActual); } if ( xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE ) { // Transform the GPS position into NED coordinates GPSVelocityData gpsVelocity; GPSVelocityGet(&gpsVelocity); VelocityActualData velocityActual; VelocityActualGet(&velocityActual); velocityActual.North = gpsVelocity.North; velocityActual.East = gpsVelocity.East; velocityActual.Down = gpsVelocity.Down; VelocityActualSet(&velocityActual); } AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); return 0; }
static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData) { float dT; portTickType thisSysTime = xTaskGetTickCount(); static portTickType lastSysTime = 0; static float accels_filtered[3] = {0,0,0}; static float grot_filtered[3] = {0,0,0}; dT = (thisSysTime == lastSysTime) ? 0.001f : TICKS2MS(portMAX_DELAY & (thisSysTime - lastSysTime)) / 1000.0f; lastSysTime = thisSysTime; // Bad practice to assume structure order, but saves memory float * gyros = &gyrosData->x; float * accels = &accelsData->x; float grot[3]; float accel_err[3]; // Apply smoothing to accel values, to reduce vibration noise before main calculations. apply_accel_filter(accels,accels_filtered); // Rotate gravity to body frame, filter and cross with accels grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); // Apply same filtering to the rotated attitude to match delays apply_accel_filter(grot,grot_filtered); // Compute the error between the predicted direction of gravity and smoothed acceleration CrossProduct((const float *) accels_filtered, (const float *) grot_filtered, accel_err); // Account for accel magnitude float accel_mag = sqrtf(accels_filtered[0]*accels_filtered[0] + accels_filtered[1]*accels_filtered[1] + accels_filtered[2]*accels_filtered[2]); // Account for filtered gravity vector magnitude float grot_mag; if (accel_filter_enabled) grot_mag = sqrtf(grot_filtered[0]*grot_filtered[0] + grot_filtered[1]*grot_filtered[1] + grot_filtered[2]*grot_filtered[2]); else grot_mag = 1.0f; if (grot_mag > 1.0e-3f && accel_mag > 1.0e-3f) { accel_err[0] /= (accel_mag*grot_mag); accel_err[1] /= (accel_mag*grot_mag); accel_err[2] /= (accel_mag*grot_mag); // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s gyro_correct_int[0] -= accel_err[0] * accelKi; gyro_correct_int[1] -= accel_err[1] * accelKi; // Correct rates based on error, integral component dealt with in updateSensors gyros[0] += accel_err[0] * accelKp / dT; gyros[1] += accel_err[1] * accelKp / dT; gyros[2] += accel_err[2] * accelKp / dT; } { // scoping variables to save memory // Work out time derivative from INSAlgo writeup // Also accounts for the fact that gyros are in deg/s float qdot[4]; qdot[0] = (-q[1] * gyros[0] - q[2] * gyros[1] - q[3] * gyros[2]) * dT * DEG2RAD / 2; qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * DEG2RAD / 2; qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * DEG2RAD / 2; qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * gyros[2]) * dT * DEG2RAD / 2; // Take a time step q[0] = q[0] + qdot[0]; q[1] = q[1] + qdot[1]; q[2] = q[2] + qdot[2]; q[3] = q[3] + qdot[3]; if(q[0] < 0) { q[0] = -q[0]; q[1] = -q[1]; q[2] = -q[2]; q[3] = -q[3]; } } // Renomalize float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); q[0] = q[0] / qmag; q[1] = q[1] / qmag; q[2] = q[2] / qmag; q[3] = q[3] / qmag; // If quaternion has become inappropriately short or is nan reinit. // THIS SHOULD NEVER ACTUALLY HAPPEN if((fabs(qmag) < 1e-3) || (qmag != qmag)) { q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; } AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); quat_copy(q, &attitudeActual.q1); // Convert into eueler degrees (makes assumptions about RPY order) Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll); AttitudeActualSet(&attitudeActual); }