/** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup Attitude Copter Control Attitude Estimation * @brief Acquires sensor data and computes attitude estimate * Specifically updates the the @ref AttitudeState "AttitudeState" and @ref AttitudeRaw "AttitudeRaw" settings objects * @{ * * @file attitude.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Module to handle all comms to the AHRS on a periodic basis. * * @see The GNU Public License (GPL) Version 3 * ******************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. * * You should have received a copy of the GNU General Public License along * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ /** * Input objects: None, takes sensor data via pios * Output objects: @ref AttitudeRaw @ref AttitudeState * * This module computes an attitude estimate from the sensor data * * The module executes in its own thread. * * UAVObjects are automatically generated by the UAVObjectGenerator from * the object definition XML file. * * Modules have no API, all communication to other modules is done through UAVObjects. * However modules may use the API exposed by shared libraries. * See the OpenPilot wiki for more details. * http://www.openpilot.org/OpenPilot_Application_Architecture * */ #include #include #include "attitude.h" #include "gyrostate.h" #include "accelstate.h" #include "attitudestate.h" #include "attitudesettings.h" #include "accelgyrosettings.h" #include "flightstatus.h" #include "manualcontrolcommand.h" #include "taskinfo.h" #include #include #include #include "CoordinateConversions.h" #include #include #include #include PERF_DEFINE_COUNTER(counterUpd); PERF_DEFINE_COUNTER(counterAccelSamples); PERF_DEFINE_COUNTER(counterPeriod); PERF_DEFINE_COUNTER(counterAtt); // Counters: // - 0xA7710001 sensor fetch duration // - 0xA7710002 updateAttitude execution time // - 0xA7710003 Attitude loop rate(period) // - 0xA7710004 number of accel samples read for each loop (cc only). // Private constants #define STACK_SIZE_BYTES 540 #define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // Attitude module loop interval (defined by sensor rate in pios_config.h) static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE); #define UPDATE_RATE 25.0f // Interval in number of sample to recalculate temp bias #define TEMP_CALIB_INTERVAL 30 // LPF #define TEMP_DT (1.0f / PIOS_SENSOR_RATE) #define TEMP_LPF_FC 5.0f static const float temp_alpha = TEMP_DT / (TEMP_DT + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC)); #define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE) #define UPDATE_MIN 1.0e-6f #define UPDATE_MAX 1.0f #define UPDATE_ALPHA 1.0e-2f // Private types // Private variables static xTaskHandle taskHandle; static PiOSDeltatimeConfig dtconfig; // Private functions static void AttitudeTask(void *parameters); static float gyro_correct_int[3] = { 0, 0, 0 }; static xQueueHandle gyro_queue; static int32_t updateSensors(AccelStateData *, GyroStateData *); static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData); static void updateAttitude(AccelStateData *, GyroStateData *); static void settingsUpdatedCb(UAVObjEvent *objEv); static float accelKi = 0; static float accelKp = 0; static float accel_alpha = 0; static bool accel_filter_enabled = false; static float accels_filtered[3]; static float grot_filtered[3]; static float yawBiasRate = 0; static float rollPitchBiasRate = 0.0f; static AccelGyroSettingsaccel_biasData accel_bias; static float q[4] = { 1, 0, 0, 0 }; static float R[3][3]; static int8_t rotate = 0; static bool zero_during_arming = false; static bool bias_correct_gyro = true; // static float gyros_passed[3]; // temp coefficient to calculate gyro bias static bool apply_gyro_temp = false; static bool apply_accel_temp = false; static AccelGyroSettingsgyro_temp_coeffData gyro_temp_coeff;; static AccelGyroSettingsaccel_temp_coeffData accel_temp_coeff; static AccelGyroSettingstemp_calibrated_extentData temp_calibrated_extent; static float temperature = NAN; static float accel_temp_bias[3] = { 0 }; static float gyro_temp_bias[3] = { 0 }; static uint8_t temp_calibration_count = 0; // Accel and Gyro scaling (this is the product of sensor scale and adjustement in AccelGyroSettings static AccelGyroSettingsgyro_scaleData gyro_scale; static AccelGyroSettingsaccel_scaleData accel_scale; // For running trim flights static volatile bool trim_requested = false; static volatile int32_t trim_accels[3]; static volatile int32_t trim_samples; int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535; #define STD_CC_ACCEL_SCALE (PIOS_CONST_MKS_GRAV_ACCEL_F * 0.004f) /* 0.004f is gravity / LSB */ #define STD_CC_ANALOG_GYRO_NEUTRAL 1665 #define STD_CC_ANALOG_GYRO_GAIN 0.42f static struct PIOS_SENSORS_3Axis_SensorsWithTemp *mpu6000_data = NULL; // Used to detect CC vs CC3D static const struct pios_board_info *bdinfo = &pios_board_info_blob; #define BOARDISCC3D (bdinfo->board_rev == 0x02) /** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t AttitudeStart(void) { // Start main task xTaskCreate(AttitudeTask, "Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, taskHandle); #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); #endif return 0; } /** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t AttitudeInitialize(void) { AttitudeStateInitialize(); AttitudeSettingsInitialize(); AccelGyroSettingsInitialize(); AccelStateInitialize(); GyroStateInitialize(); // Initialize quaternion AttitudeStateData attitude; AttitudeStateGet(&attitude); attitude.q1 = 1; attitude.q2 = 0; attitude.q3 = 0; attitude.q4 = 0; AttitudeStateSet(&attitude); // Cannot trust the values to init right above if BL runs gyro_correct_int[0] = 0; gyro_correct_int[1] = 0; gyro_correct_int[2] = 0; q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; for (uint8_t i = 0; i < 3; i++) { for (uint8_t j = 0; j < 3; j++) { R[i][j] = 0; } } trim_requested = false; AttitudeSettingsConnectCallback(&settingsUpdatedCb); AccelGyroSettingsConnectCallback(&settingsUpdatedCb); return 0; } MODULE_INITCALL(AttitudeInitialize, AttitudeStart); /** * Module thread, should not return. */ int32_t accel_test; int32_t gyro_test; static void AttitudeTask(__attribute__((unused)) void *parameters) { uint8_t init = 0; AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); bool cc3d = BOARDISCC3D; if (cc3d) { #if defined(PIOS_INCLUDE_MPU6000) gyro_test = PIOS_MPU6000_Driver.test(0); mpu6000_data = pios_malloc(sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + sizeof(Vector3i16) * 2); #endif } else { #if defined(PIOS_INCLUDE_ADXL345) // Set critical error and wait until the accel is producing data while (PIOS_ADXL345_FifoElements() == 0) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); #ifdef PIOS_INCLUDE_WDG PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); #endif } accel_test = PIOS_ADXL345_Test(); #endif #if defined(PIOS_INCLUDE_ADC) // Create queue for passing gyro data, allow 2 back samples in case gyro_queue = xQueueCreate(1, sizeof(float) * 4); PIOS_Assert(gyro_queue != NULL); PIOS_ADC_SetQueue(gyro_queue); PIOS_ADC_Config(46); #endif } PERF_INIT_COUNTER(counterUpd, 0xA7710001); PERF_INIT_COUNTER(counterAtt, 0xA7710002); PERF_INIT_COUNTER(counterPeriod, 0xA7710003); PERF_INIT_COUNTER(counterAccelSamples, 0xA7710004); // Force settings update to make sure rotation loaded settingsUpdatedCb(AttitudeSettingsHandle()); PIOS_DELTATIME_Init(&dtconfig, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); portTickType lastSysTime = xTaskGetTickCount(); // Main task loop while (1) { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if ((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { // Use accels to initialise attitude and calculate gyro bias accelKp = 1.0f; accelKi = 0.0f; yawBiasRate = 0.01f; rollPitchBiasRate = 0.01f; accel_filter_enabled = false; init = 0; PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR); } else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { accelKp = 1.0f; accelKi = 0.0f; yawBiasRate = 0.01f; rollPitchBiasRate = 0.01f; accel_filter_enabled = false; init = 0; PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR); } else if (init == 0) { // Reload settings (all the rates) AttitudeSettingsAccelKiGet(&accelKi); AttitudeSettingsAccelKpGet(&accelKp); AttitudeSettingsYawBiasRateGet(&yawBiasRate); rollPitchBiasRate = 0.0f; if (accel_alpha > 0.0f) { accel_filter_enabled = true; } init = 1; } #ifdef PIOS_INCLUDE_WDG PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); #endif AccelStateData accelState; GyroStateData gyros; int32_t retval = 0; if (cc3d) { retval = updateSensorsCC3D(&accelState, &gyros); } else { retval = updateSensors(&accelState, &gyros); } // Only update attitude when sensor data is good if (retval != 0) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); } else { // Do not update attitude data in simulation mode if (!AttitudeStateReadOnly()) { PERF_TIMED_SECTION_START(counterAtt); updateAttitude(&accelState, &gyros); PERF_TIMED_SECTION_END(counterAtt); } PERF_MEASURE_PERIOD(counterPeriod); AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); } vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_PERIOD_MS); } } /** * Get an update from the sensors * @param[in] attitudeRaw Populate the UAVO instead of saving right here * @return 0 if successfull, -1 if not */ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros) { struct pios_adxl345_data accel_data; float gyro[4]; // Only wait the time for two nominal updates before setting an alarm if (xQueueReceive(gyro_queue, (void *const)gyro, UPDATE_RATE * 2) == errQUEUE_EMPTY) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); return -1; } // Do not read raw sensor data in simulation mode if (GyroStateReadOnly() || AccelStateReadOnly()) { return 0; } // No accel data available uint8_t fifoSamples = PIOS_ADXL345_FifoElements(); if (fifoSamples == 0) { return -1; } PERF_TIMED_SECTION_START(counterUpd); // First sample is temperature gyros->x = -(gyro[1] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.X; gyros->y = (gyro[2] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.Y; gyros->z = -(gyro[3] - STD_CC_ANALOG_GYRO_NEUTRAL) * gyro_scale.Z; int32_t x = 0; int32_t y = 0; int32_t z = 0; uint8_t i = fifoSamples; uint8_t samples_remaining; samples_remaining = PIOS_ADXL345_ReadAndAccumulateSamples(&accel_data, fifoSamples); x = accel_data.x; y = -accel_data.y; z = -accel_data.z; if (samples_remaining > 0) { do { i++; samples_remaining = PIOS_ADXL345_Read(&accel_data); x += accel_data.x; y += -accel_data.y; z += -accel_data.z; } while ((i < 32) && (samples_remaining > 0)); } PERF_TRACK_VALUE(counterAccelSamples, i); float accel[3] = { accel_scale.X * (float)x / i, accel_scale.Y * (float)y / i, accel_scale.Z * (float)z / i }; if (rotate) { // TODO: rotate sensors too so stabilization is well behaved float vec_out[3]; rot_mult(R, accel, vec_out); accelState->x = vec_out[0]; accelState->y = vec_out[1]; accelState->z = vec_out[2]; rot_mult(R, &gyros->x, vec_out); gyros->x = vec_out[0]; gyros->y = vec_out[1]; gyros->z = vec_out[2]; } else { accelState->x = accel[0]; accelState->y = accel[1]; accelState->z = accel[2]; } if (trim_requested) { if (trim_samples >= MAX_TRIM_FLIGHT_SAMPLES) { trim_requested = false; } else { uint8_t armed; float throttle; FlightStatusArmedGet(&armed); ManualControlCommandThrottleGet(&throttle); // Until flight status indicates airborne if ((armed == FLIGHTSTATUS_ARMED_ARMED) && (throttle > 0.0f)) { trim_samples++; // Store the digitally scaled version since that is what we use for bias trim_accels[0] += accelState->x; trim_accels[1] += accelState->y; trim_accels[2] += accelState->z; } } } // Scale accels and correct bias accelState->x -= accel_bias.X; accelState->y -= accel_bias.Y; accelState->z -= accel_bias.Z; if (bias_correct_gyro) { // Applying integral component here so it can be seen on the gyros and correct bias gyros->x += gyro_correct_int[0]; gyros->y += gyro_correct_int[1]; gyros->z += gyro_correct_int[2]; } // Force the roll & pitch gyro rates to average to zero during initialisation gyro_correct_int[0] += -gyros->x * rollPitchBiasRate; gyro_correct_int[1] += -gyros->y * rollPitchBiasRate; // Because most crafts wont get enough information from gravity to zero yaw gyro, we try // and make it average zero (weakly) gyro_correct_int[2] += -gyros->z * yawBiasRate; PERF_TIMED_SECTION_END(counterUpd); GyroStateSet(gyros); AccelStateSet(accelState); return 0; } /** * Get an update from the sensors * @param[in] attitudeRaw Populate the UAVO instead of saving right here * @return 0 if successfull, -1 if not */ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData) { float accels[3] = { 0 }; float gyros[3] = { 0 }; float temp = 0; uint8_t count = 0; #if defined(PIOS_INCLUDE_MPU6000) xQueueHandle queue = PIOS_MPU6000_Driver.get_queue(0); BaseType_t ret = xQueueReceive(queue, (void *)mpu6000_data, sensor_period_ms); while (ret == pdTRUE) { gyros[0] += mpu6000_data->sample[1].x; gyros[1] += mpu6000_data->sample[1].y; gyros[2] += mpu6000_data->sample[1].z; accels[0] += mpu6000_data->sample[0].x; accels[1] += mpu6000_data->sample[0].y; accels[2] += mpu6000_data->sample[0].z; temp += mpu6000_data->temperature; count++; // check if further samples are already in queue ret = xQueueReceive(queue, (void *)mpu6000_data, 0); } PERF_TRACK_VALUE(counterAccelSamples, count); if (!count) { return -1; // Error, no data } // Do not read raw sensor data in simulation mode if (GyroStateReadOnly() || AccelStateReadOnly()) { return 0; } float invcount = 1.0f / count; PERF_TIMED_SECTION_START(counterUpd); gyros[0] *= gyro_scale.X * invcount; gyros[1] *= gyro_scale.Y * invcount; gyros[2] *= gyro_scale.Z * invcount; accels[0] *= accel_scale.X * invcount; accels[1] *= accel_scale.Y * invcount; accels[2] *= accel_scale.Z * invcount; temp *= invcount; if (isnan(temperature)) { temperature = temp; } temperature = temp_alpha * (temp - temperature) + temperature; if ((apply_gyro_temp || apply_accel_temp) && !temp_calibration_count) { temp_calibration_count = TEMP_CALIB_INTERVAL; float ctemp = boundf(temperature, temp_calibrated_extent.max, temp_calibrated_extent.min); if (apply_gyro_temp) { gyro_temp_bias[0] = (gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp) * ctemp; gyro_temp_bias[1] = (gyro_temp_coeff.Y + gyro_temp_coeff.Y2 * ctemp) * ctemp; gyro_temp_bias[2] = (gyro_temp_coeff.Z + gyro_temp_coeff.Z2 * ctemp) * ctemp; } if (apply_accel_temp) { accel_temp_bias[0] = accel_temp_coeff.X * ctemp; accel_temp_bias[1] = accel_temp_coeff.Y * ctemp; accel_temp_bias[2] = accel_temp_coeff.Z * ctemp; } } temp_calibration_count--; if (apply_gyro_temp) { gyros[0] -= gyro_temp_bias[0]; gyros[1] -= gyro_temp_bias[1]; gyros[2] -= gyro_temp_bias[2]; } if (apply_accel_temp) { accels[0] -= accel_temp_bias[0]; accels[1] -= accel_temp_bias[1]; accels[2] -= accel_temp_bias[2]; } // gyrosData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; // accelsData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; #endif /* if defined(PIOS_INCLUDE_MPU6000) */ if (rotate) { // TODO: rotate sensors too so stabilization is well behaved float vec_out[3]; rot_mult(R, accels, vec_out); accels[0] = vec_out[0]; accels[1] = vec_out[1]; accels[2] = vec_out[2]; rot_mult(R, gyros, vec_out); gyros[0] = vec_out[0]; gyros[1] = vec_out[1]; gyros[2] = vec_out[2]; } accelStateData->x = accels[0] - accel_bias.X; accelStateData->y = accels[1] - accel_bias.Y; accelStateData->z = accels[2] - accel_bias.Z; gyrosData->x = gyros[0]; gyrosData->y = gyros[1]; gyrosData->z = gyros[2]; if (bias_correct_gyro) { // Applying integral component here so it can be seen on the gyros and correct bias gyrosData->x += gyro_correct_int[0]; gyrosData->y += gyro_correct_int[1]; gyrosData->z += gyro_correct_int[2]; } // Force the roll & pitch gyro rates to average to zero during initialisation gyro_correct_int[0] += -gyrosData->x * rollPitchBiasRate; gyro_correct_int[1] += -gyrosData->y * rollPitchBiasRate; // Because most crafts wont get enough information from gravity to zero yaw gyro, we try // and make it average zero (weakly) gyro_correct_int[2] += -gyrosData->z * yawBiasRate; PERF_TIMED_SECTION_END(counterUpd); GyroStateSet(gyrosData); AccelStateSet(accelStateData); return 0; } static inline void apply_accel_filter(const float *raw, float *filtered) { if (accel_filter_enabled) { filtered[0] = filtered[0] * accel_alpha + raw[0] * (1 - accel_alpha); filtered[1] = filtered[1] * accel_alpha + raw[1] * (1 - accel_alpha); filtered[2] = filtered[2] * accel_alpha + raw[2] * (1 - accel_alpha); } else { filtered[0] = raw[0]; filtered[1] = raw[1]; filtered[2] = raw[2]; } } __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosData) { float dT = PIOS_DELTATIME_GetAverageSeconds(&dtconfig); // Bad practice to assume structure order, but saves memory float *gyros = &gyrosData->x; float *accels = &accelStateData->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 unit vector 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_accel_filter(grot, grot_filtered); CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err); // Account for accel magnitude float inv_accel_mag = invsqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]); if (inv_accel_mag > 1e3f) { return; } // Account for filtered gravity vector magnitude float inv_grot_mag; if (accel_filter_enabled) { inv_grot_mag = invsqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]); } else { inv_grot_mag = 1.0f; } if (inv_grot_mag > 1e3f) { return; } const float invMag = (inv_accel_mag * inv_grot_mag); accel_err[0] *= invMag; accel_err[1] *= invMag; accel_err[2] *= invMag; // 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] * accelKi; // Correct rates based on error, integral component dealt with in updateSensors const float kpInvdT = accelKp / dT; gyros[0] += accel_err[0] * kpInvdT; gyros[1] += accel_err[1] * kpInvdT; gyros[2] += accel_err[2] * kpInvdT; { // 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 * (M_PI_F / 180.0f / 2.0f); qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * gyros[2]) * dT * (M_PI_F / 180.0f / 2.0f); // 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]; } } // Renormalize float inv_qmag = invsqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); // If quaternion has become inappropriately short or is nan reinit. // THIS SHOULD NEVER ACTUALLY HAPPEN if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) { q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; } else { q[0] = q[0] * inv_qmag; q[1] = q[1] * inv_qmag; q[2] = q[2] * inv_qmag; q[3] = q[3] * inv_qmag; } AttitudeStateData attitudeState; AttitudeStateGet(&attitudeState); quat_copy(q, &attitudeState.q1); // Convert into eueler degrees (makes assumptions about RPY order) Quaternion2RPY(&attitudeState.q1, &attitudeState.Roll); AttitudeStateSet(&attitudeState); } static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) { AttitudeSettingsData attitudeSettings; AccelGyroSettingsData accelGyroSettings; AttitudeSettingsGet(&attitudeSettings); AccelGyroSettingsGet(&accelGyroSettings); accelKp = attitudeSettings.AccelKp; accelKi = attitudeSettings.AccelKi; yawBiasRate = attitudeSettings.YawBiasRate; // Calculate accel filter alpha, in the same way as for gyro data in stabilization module. const float fakeDt = 0.0025f; if (attitudeSettings.AccelTau < 0.0001f) { accel_alpha = 0; // not trusting this to resolve to 0 accel_filter_enabled = false; } else { accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau); accel_filter_enabled = true; } zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE; bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE; memcpy(&gyro_temp_coeff, &accelGyroSettings.gyro_temp_coeff, sizeof(AccelGyroSettingsgyro_temp_coeffData)); memcpy(&accel_temp_coeff, &accelGyroSettings.accel_temp_coeff, sizeof(AccelGyroSettingsaccel_temp_coeffData)); apply_gyro_temp = (fabsf(gyro_temp_coeff.X) > 1e-6f || fabsf(gyro_temp_coeff.Y) > 1e-6f || fabsf(gyro_temp_coeff.Z) > 1e-6f || fabsf(gyro_temp_coeff.X2) > 1e-6f || fabsf(gyro_temp_coeff.Y2) > 1e-6f || fabsf(gyro_temp_coeff.Z2) > 1e-6f); apply_accel_temp = (fabsf(accel_temp_coeff.X) > 1e-6f || fabsf(accel_temp_coeff.Y) > 1e-6f || fabsf(accel_temp_coeff.Z) > 1e-6f); gyro_correct_int[0] = accelGyroSettings.gyro_bias.X; gyro_correct_int[1] = accelGyroSettings.gyro_bias.Y; gyro_correct_int[2] = accelGyroSettings.gyro_bias.Z; temp_calibrated_extent.min = accelGyroSettings.temp_calibrated_extent.min; temp_calibrated_extent.max = accelGyroSettings.temp_calibrated_extent.max; if (BOARDISCC3D) { float scales[2]; PIOS_MPU6000_Driver.get_scale(scales, 2, 0); accel_bias.X = accelGyroSettings.accel_bias.X; accel_bias.Y = accelGyroSettings.accel_bias.Y; accel_bias.Z = accelGyroSettings.accel_bias.Z; gyro_scale.X = accelGyroSettings.gyro_scale.X * scales[1]; gyro_scale.Y = accelGyroSettings.gyro_scale.Y * scales[1]; gyro_scale.Z = accelGyroSettings.gyro_scale.Z * scales[1]; accel_scale.X = accelGyroSettings.accel_scale.X * scales[0]; accel_scale.Y = accelGyroSettings.accel_scale.Y * scales[0]; accel_scale.Z = accelGyroSettings.accel_scale.Z * scales[0]; } else { // Original CC with analog gyros and ADXL accel accel_bias.X = accelGyroSettings.accel_bias.X; accel_bias.Y = accelGyroSettings.accel_bias.Y; accel_bias.Z = accelGyroSettings.accel_bias.Z; gyro_scale.X = accelGyroSettings.gyro_scale.X * STD_CC_ANALOG_GYRO_GAIN; gyro_scale.Y = accelGyroSettings.gyro_scale.Y * STD_CC_ANALOG_GYRO_GAIN; gyro_scale.Z = accelGyroSettings.gyro_scale.Z * STD_CC_ANALOG_GYRO_GAIN; accel_scale.X = accelGyroSettings.accel_scale.X * STD_CC_ACCEL_SCALE; accel_scale.Y = accelGyroSettings.accel_scale.Y * STD_CC_ACCEL_SCALE; accel_scale.Z = accelGyroSettings.accel_scale.Z * STD_CC_ACCEL_SCALE; } // Indicates not to expend cycles on rotation if (fabsf(attitudeSettings.BoardRotation.Pitch) < 0.00001f && fabsf(attitudeSettings.BoardRotation.Roll) < 0.00001f && fabsf(attitudeSettings.BoardRotation.Yaw) < 0.00001f) { rotate = 0; // Shouldn't be used but to be safe float rotationQuat[4] = { 1, 0, 0, 0 }; Quaternion2R(rotationQuat, R); } else { float rotationQuat[4]; const float rpy[3] = { attitudeSettings.BoardRotation.Roll, attitudeSettings.BoardRotation.Pitch, attitudeSettings.BoardRotation.Yaw }; RPY2Quaternion(rpy, rotationQuat); Quaternion2R(rotationQuat, R); rotate = 1; } if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_START) { trim_accels[0] = 0; trim_accels[1] = 0; trim_accels[2] = 0; trim_samples = 0; trim_requested = true; } else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) { trim_requested = false; accelGyroSettings.accel_scale.X = trim_accels[0] / trim_samples; accelGyroSettings.accel_scale.Y = trim_accels[1] / trim_samples; // Z should average -grav accelGyroSettings.accel_scale.Z = trim_accels[2] / trim_samples + PIOS_CONST_MKS_GRAV_ACCEL_F; attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL; AttitudeSettingsSet(&attitudeSettings); } else { trim_requested = false; } } /** * @} * @} */