static void settingsUpdatedCb(UAVObjEvent * objEv) {
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);
accelKp = attitudeSettings.AccelKp;
accelKi = attitudeSettings.AccelKi;
gyroGain = attitudeSettings.GyroGain;
accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X];
accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y];
accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z];
}
/**
* Locally cache some variables from the AtttitudeSettings object
*/
static void settingsUpdatedCb(UAVObjEvent * objEv) {
RevoCalibrationGet(&cal);
mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X];
mag_bias[1] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Y];
mag_bias[2] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Z];
mag_scale[0] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_X];
mag_scale[1] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Y];
mag_scale[2] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Z];
accel_bias[0] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_X];
accel_bias[1] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Y];
accel_bias[2] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Z];
accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X];
accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y];
accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z];
// Do not store gyros_bias here as that comes from the state estimator and should be
// used from GyroBias directly
// Zero out any adaptive tracking
MagBiasData magBias;
MagBiasGet(&magBias);
magBias.x = 0;
magBias.y = 0;
magBias.z = 0;
MagBiasSet(&magBias);
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);
bias_correct_gyro = (cal.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE);
// Indicates not to expend cycles on rotation
if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
attitudeSettings.BoardRotation[2] == 0) {
rotate = 0;
} else {
float rotationQuat[4];
const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL],
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH],
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]};
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R);
rotate = 1;
}
}
static void settingsUpdatedCb(UAVObjEvent * objEv) {
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);
accelKp = attitudeSettings.AccelKp;
accelKi = attitudeSettings.AccelKi;
yawBiasRate = attitudeSettings.YawBiasRate;
gyroGain = attitudeSettings.GyroGain;
zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE;
accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X];
accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y];
accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z];
gyro_correct_int[0] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f;
gyro_correct_int[1] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f;
gyro_correct_int[2] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f;
// Indicates not to expend cycles on rotation
if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
attitudeSettings.BoardRotation[2] == 0) {
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[ATTITUDESETTINGS_BOARDROTATION_ROLL],
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH],
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]};
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R);
rotate = 1;
}
}
static void settingsUpdatedCb(UAVObjEvent * ev)
{
if (ev == NULL || ev->obj == RevoCalibrationHandle()) {
RevoCalibrationGet(&revoCalibration);
/* When the revo calibration is updated, update the GyroBias object */
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosBias.x = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
gyrosBias.y = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y];
gyrosBias.z = revoCalibration.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z];
GyrosBiasSet(&gyrosBias);
gyroBiasSettingsUpdated = true;
// In case INS currently running
INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
}
if(ev == NULL || ev->obj == HomeLocationHandle()) {
HomeLocationGet(&homeLocation);
// Compute matrix to convert deltaLLA to NED
float lat, alt;
lat = homeLocation.Latitude / 10.0e6f * DEG2RAD;
alt = homeLocation.Altitude;
T[0] = alt+6.378137E6f;
T[1] = cosf(lat)*(alt+6.378137E6f);
T[2] = -1.0f;
}
if (ev == NULL || ev->obj == AttitudeSettingsHandle())
AttitudeSettingsGet(&attitudeSettings);
if (ev == NULL || ev->obj == RevoSettingsHandle())
RevoSettingsGet(&revoSettings);
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t TxPIDInitialize(void)
{
bool txPIDEnabled;
HwSettingsOptionalModulesData optionalModules;
#ifdef REVOLUTION
AltitudeHoldSettingsInitialize();
#endif
HwSettingsInitialize();
HwSettingsOptionalModulesGet(&optionalModules);
if (optionalModules.TxPID == HWSETTINGS_OPTIONALMODULES_ENABLED) {
txPIDEnabled = true;
} else {
txPIDEnabled = false;
}
if (txPIDEnabled) {
TxPIDSettingsInitialize();
TxPIDStatusInitialize();
AccessoryDesiredInitialize();
UAVObjEvent ev = {
.obj = AccessoryDesiredHandle(),
.instId = 0,
.event = 0,
.lowPriority = false,
};
EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
#if (TELEMETRY_UPDATE_PERIOD_MS != 0)
// Change StabilizationSettings update rate from OnChange to periodic
// to prevent telemetry link flooding with frequent updates in case of
// control channel jitter.
// Warning: saving to flash with this code active will change the
// StabilizationSettings update rate permanently. Use Metadata via
// browser to reset to defaults (telemetryAcked=true, OnChange).
UAVObjMetadata metadata;
StabilizationSettingsInitialize();
StabilizationSettingsGetMetadata(&metadata);
metadata.telemetryAcked = 0;
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
StabilizationSettingsSetMetadata(&metadata);
AttitudeSettingsInitialize();
AttitudeSettingsGetMetadata(&metadata);
metadata.telemetryAcked = 0;
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
AttitudeSettingsSetMetadata(&metadata);
#endif /* if (TELEMETRY_UPDATE_PERIOD_MS != 0) */
return 0;
}
return -1;
}
/* stub: module has no module thread */
int32_t TxPIDStart(void)
{
return 0;
}
MODULE_INITCALL(TxPIDInitialize, TxPIDStart);
/**
* Update PIDs callback function
*/
static void updatePIDs(UAVObjEvent *ev)
{
if (ev->obj != AccessoryDesiredHandle()) {
return;
}
TxPIDSettingsData inst;
TxPIDSettingsGet(&inst);
if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER) {
return;
}
uint8_t armed;
FlightStatusArmedGet(&armed);
if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) &&
(armed == FLIGHTSTATUS_ARMED_DISARMED)) {
return;
}
StabilizationBankData bank;
switch (inst.BankNumber) {
case 0:
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
break;
case 1:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
break;
case 2:
StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&bank);
break;
default:
return;
}
StabilizationSettingsData stab;
StabilizationSettingsGet(&stab);
AttitudeSettingsData att;
AttitudeSettingsGet(&att);
#ifdef REVOLUTION
AltitudeHoldSettingsData altitude;
AltitudeHoldSettingsGet(&altitude);
#endif
AccessoryDesiredData accessory;
TxPIDStatusData txpid_status;
TxPIDStatusGet(&txpid_status);
bool easyTuneEnabled = false;
uint8_t needsUpdateBank = 0;
uint8_t needsUpdateStab = 0;
uint8_t needsUpdateAtt = 0;
#ifdef REVOLUTION
uint8_t needsUpdateAltitude = 0;
#endif
// Loop through every enabled instance
for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
if (TxPIDSettingsPIDsToArray(inst.PIDs)[i] != TXPIDSETTINGS_PIDS_DISABLED) {
float value;
if (TxPIDSettingsInputsToArray(inst.Inputs)[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
ManualControlCommandThrottleGet(&value);
value = scale(value,
inst.ThrottleRange.Min,
inst.ThrottleRange.Max,
TxPIDSettingsMinPIDToArray(inst.MinPID)[i],
TxPIDSettingsMaxPIDToArray(inst.MaxPID)[i]);
} else if (AccessoryDesiredInstGet(
TxPIDSettingsInputsToArray(inst.Inputs)[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0,
&accessory) == 0) {
value = scale(accessory.AccessoryVal, -1.0f, 1.0f,
TxPIDSettingsMinPIDToArray(inst.MinPID)[i],
TxPIDSettingsMaxPIDToArray(inst.MaxPID)[i]);
} else {
continue;
}
TxPIDStatusCurPIDToArray(txpid_status.CurPID)[i] = value;
switch (TxPIDSettingsPIDsToArray(inst.PIDs)[i]) {
case TXPIDSETTINGS_PIDS_ROLLRATEKP:
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_EASYTUNERATEROLL:
easyTuneEnabled = true;
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
needsUpdateBank |= update(&bank.RollRatePID.Ki, value * inst.EasyTunePitchRollRateFactors.I);
needsUpdateBank |= update(&bank.RollRatePID.Kd, value * inst.EasyTunePitchRollRateFactors.D);
break;
case TXPIDSETTINGS_PIDS_EASYTUNERATEPITCH:
easyTuneEnabled = true;
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value * inst.EasyTunePitchRollRateFactors.I);
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value * inst.EasyTunePitchRollRateFactors.D);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKI:
needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKD:
needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATERESP:
needsUpdateBank |= updateUint16(&bank.ManualRate.Roll, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
needsUpdateBank |= update(&bank.RollPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
needsUpdateBank |= update(&bank.RollPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.RollMax, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKP:
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKI:
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKD:
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATERESP:
needsUpdateBank |= updateUint16(&bank.ManualRate.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
needsUpdateBank |= update(&bank.PitchPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.PitchMax, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATERESP:
needsUpdateBank |= updateUint16(&bank.ManualRate.Roll, value);
needsUpdateBank |= updateUint16(&bank.ManualRate.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
needsUpdateBank |= update(&bank.RollPI.Kp, value);
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
needsUpdateBank |= update(&bank.RollPI.Ki, value);
needsUpdateBank |= update(&bank.PitchPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.RollMax, value);
needsUpdateBank |= updateUint8(&bank.PitchMax, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKP:
needsUpdateBank |= update(&bank.YawRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKI:
needsUpdateBank |= update(&bank.YawRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKD:
needsUpdateBank |= update(&bank.YawRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
needsUpdateBank |= update(&bank.YawRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATERESP:
needsUpdateBank |= updateUint16(&bank.ManualRate.Yaw, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
needsUpdateBank |= update(&bank.YawPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
needsUpdateBank |= update(&bank.YawPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
needsUpdateBank |= update(&bank.YawPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.YawMax, value);
break;
case TXPIDSETTINGS_PIDS_ROLLEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value);
break;
case TXPIDSETTINGS_PIDS_PITCHEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value);
needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_YAWEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Yaw, value);
break;
case TXPIDSETTINGS_PIDS_GYROTAU:
needsUpdateStab |= update(&stab.GyroTau, value);
break;
case TXPIDSETTINGS_PIDS_ACROROLLFACTOR:
needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Roll, value);
break;
case TXPIDSETTINGS_PIDS_ACROPITCHFACTOR:
needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_ACROROLLPITCHFACTOR:
needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Roll, value);
needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_ACCELTAU:
needsUpdateAtt |= update(&att.AccelTau, value);
break;
case TXPIDSETTINGS_PIDS_ACCELKP:
needsUpdateAtt |= update(&att.AccelKp, value);
break;
case TXPIDSETTINGS_PIDS_ACCELKI:
needsUpdateAtt |= update(&att.AccelKi, value);
break;
#ifdef REVOLUTION
case TXPIDSETTINGS_PIDS_ALTITUDEPOSKP:
needsUpdateAltitude |= update(&altitude.VerticalPosP, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKP:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKI:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKD:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYBETA:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Beta, value);
break;
#endif
default:
PIOS_Assert(0);
}
}
}
if (needsUpdateStab) {
StabilizationSettingsSet(&stab);
}
if (needsUpdateAtt) {
AttitudeSettingsSet(&att);
}
#ifdef REVOLUTION
if (needsUpdateAltitude) {
AltitudeHoldSettingsSet(&altitude);
}
#endif
if (easyTuneEnabled && (inst.EasyTuneRatePIDRecalculateYaw != TXPIDSETTINGS_EASYTUNERATEPIDRECALCULATEYAW_FALSE)) {
float newKp = (bank.RollRatePID.Kp + bank.PitchRatePID.Kp) * .5f * inst.EasyTuneYawRateFactors.P;
needsUpdateBank |= update(&bank.YawRatePID.Kp, newKp);
needsUpdateBank |= update(&bank.YawRatePID.Ki, newKp * inst.EasyTuneYawRateFactors.I);
needsUpdateBank |= update(&bank.YawRatePID.Kd, newKp * inst.EasyTuneYawRateFactors.D);
}
if (needsUpdateBank) {
switch (inst.BankNumber) {
case 0:
StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *)&bank);
break;
case 1:
StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *)&bank);
break;
case 2:
StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *)&bank);
break;
default:
return;
}
}
if (needsUpdateStab ||
needsUpdateAtt ||
#ifdef REVOLUTION
needsUpdateAltitude ||
#endif /* REVOLUTION */
needsUpdateBank) {
TxPIDStatusSet(&txpid_status);;
}
}
/**
* Scales input val from [inMin..inMax] range to [outMin..outMax].
* If val is out of input range (inMin <= inMax), it will be bound.
* (outMin > outMax) is ok, in that case output will be decreasing.
*
* \returns scaled value
*/
static float scale(float val, float inMin, float inMax, float outMin, float outMax)
{
// bound input value
if (val > inMax) {
val = inMax;
}
if (val < inMin) {
val = inMin;
}
// normalize input value to [0..1]
if (inMax <= inMin) {
val = 0.0f;
} else {
val = (val - inMin) / (inMax - inMin);
}
// update output bounds
if (outMin > outMax) {
float t = outMin;
outMin = outMax;
outMax = t;
val = 1.0f - val;
}
return (outMax - outMin) * val + outMin;
}
/**
* Updates var using val if needed.
* \returns 1 if updated, 0 otherwise
*/
static uint8_t update(float *var, float val)
{
/* FIXME: this is not an entirely correct way
* to check if the two floating point
* numbers are 'not equal'.
* Epsilon of 1e-9 is probably okay for the range
* of numbers we see here*/
if (fabsf(*var - val) > 1e-9f) {
*var = val;
return 1;
}
return 0;
}
/**
* Updates var using val if needed.
* \returns 1 if updated, 0 otherwise
*/
static uint8_t updateUint16(uint16_t *var, float val)
{
uint16_t roundedVal = (uint16_t)roundf(val);
if (*var != roundedVal) {
*var = roundedVal;
return 1;
}
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 settingsUpdatedCb(UAVObjEvent * objEv) {
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);
SensorSettingsGet(&sensorSettings);
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;
gyro_correct_int[0] = 0;
gyro_correct_int[1] = 0;
gyro_correct_int[2] = 0;
// Indicates not to expend cycles on rotation
if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
attitudeSettings.BoardRotation[2] == 0) {
rotate = 0;
// Shouldn't be used but to be safe
float rotationQuat[4] = {1,0,0,0};
Quaternion2R(rotationQuat, Rsb);
} else {
float rotationQuat[4];
const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL] / 100.0f,
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH] / 100.0f,
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW] / 100.0f
};
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, Rsb);
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;
// Get sensor data mean
float a_body[3] = { trim_accels[0] / trim_samples,
trim_accels[1] / trim_samples,
trim_accels[2] / trim_samples
};
// Inverse rotation of sensor data, from body frame into sensor frame
float a_sensor[3];
rot_mult(Rsb, a_body, a_sensor, false);
// Temporary variables
float psi, theta, phi;
psi = attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW] * DEG2RAD / 100.0f;
float cP = cosf(psi);
float sP = sinf(psi);
// In case psi is too small, we have to use a different equation to solve for theta
if (fabsf(psi) > PI / 2)
theta = atanf((a_sensor[1] + cP * (sP * a_sensor[0] -
cP * a_sensor[1])) / (sP * a_sensor[2]));
else
theta = atanf((a_sensor[0] - sP * (sP * a_sensor[0] -
cP * a_sensor[1])) / (cP * a_sensor[2]));
phi = atan2f((sP * a_sensor[0] - cP * a_sensor[1]) / GRAVITY,
(a_sensor[2] / cosf(theta) / GRAVITY));
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL] = phi * RAD2DEG * 100.0f;
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH] = theta * RAD2DEG * 100.0f;
attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
AttitudeSettingsSet(&attitudeSettings);
}
}
