/**
* @brief Gimbal output control task
*/
static void brushlessGimbalTask(void* parameters)
{
UAVObjEvent ev;
TIM2->CNT = 0;
TIM3->CNT = 0;
TIM15->CNT = 0;
TIM17->CNT = 0;
bool armed = false;
bool previous_armed = false;
while (1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR);
// Wait until the ActuatorDesired object is updated
PIOS_Queue_Receive(queue, &ev, 1);
previous_armed = armed;
armed |= PIOS_Thread_Systime() > 10000;
if (armed && !previous_armed) {
PIOS_Brushless_SetUpdateRate(60000);
}
if (!armed)
continue;
ActuatorDesiredData actuatorDesired;
ActuatorDesiredGet(&actuatorDesired);
// Set the rotation in electrical degrees per second. Note these
// will be divided by the number of physical poles to get real
// mechanical degrees per second
BrushlessGimbalSettingsData settings;
BrushlessGimbalSettingsGet(&settings);
PIOS_Brushless_SetScale(settings.PowerScale[0], settings.PowerScale[1], settings.PowerScale[2]);
PIOS_Brushless_SetMaxAcceleration(settings.SlewLimit[0], settings.SlewLimit[1], settings.SlewLimit[2]);
PIOS_Brushless_SetSpeed(0, actuatorDesired.Roll * settings.MaxDPS[BRUSHLESSGIMBALSETTINGS_MAXDPS_ROLL], 0.001f);
PIOS_Brushless_SetSpeed(1, actuatorDesired.Pitch * settings.MaxDPS[BRUSHLESSGIMBALSETTINGS_MAXDPS_PITCH], 0.001f);
// Use the gyros to set a damping term. This creates a phase offset of the integrated
// driving position to make the control pull against any momentum. Essentially the main
// output to the driver (above) is a velocity signal which the driver takes care of
// integrating to create a position. The current rate of roll creates a shift in that
// position (without changing the integrated position).
// This idea was taken from https://code.google.com/p/brushless-gimbal/
GyrosData gyros;
GyrosGet(&gyros);
PIOS_Brushless_SetPhaseLag(0, -gyros.x * settings.Damping[BRUSHLESSGIMBALSETTINGS_DAMPING_ROLL]);
PIOS_Brushless_SetPhaseLag(1, -gyros.y * settings.Damping[BRUSHLESSGIMBALSETTINGS_DAMPING_PITCH]);
}
}
void Gyros_Get(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
{
if (Param[0]->Val->Pointer == NULL)
return;
GyrosData data;
GyrosGet(&data);
// use the same struct like picoc. see below
struct mystruct {
double x;
double y;
double z;
double temperature;
} *pdata;
pdata = Param[0]->Val->Pointer;
pdata->x = data.x;
pdata->y = data.y;
pdata->z = data.z;
pdata->temperature = data.temperature;
}
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;
}
/**
* @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;
}
/**
* Module task
*/
static void stabilizationTask(void* parameters)
{
UAVObjEvent ev;
uint32_t timeval = PIOS_DELAY_GetRaw();
ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired;
RateDesiredData rateDesired;
AttitudeActualData attitudeActual;
GyrosData gyrosData;
FlightStatusData flightStatus;
float *stabDesiredAxis = &stabDesired.Roll;
float *actuatorDesiredAxis = &actuatorDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll;
float horizonRateFraction = 0.0f;
// Force refresh of all settings immediately before entering main task loop
SettingsUpdatedCb((UAVObjEvent *) NULL);
// Settings for system identification
uint32_t iteration = 0;
const uint32_t SYSTEM_IDENT_PERIOD = 75;
uint32_t system_ident_timeval = PIOS_DELAY_GetRaw();
float dT_filtered = 0;
// Main task loop
zero_pids();
while(1) {
iteration++;
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if (PIOS_Queue_Receive(queue, &ev, FAILSAFE_TIMEOUT_MS) != true)
{
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
continue;
}
calculate_pids();
float dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
timeval = PIOS_DELAY_GetRaw();
// exponential moving averaging (EMA) of dT to reduce jitter; ~200points
// to have more or less equivalent noise reduction to a normal N point moving averaging: alpha = 2 / (N + 1)
// run it only at the beginning for the first samples, to reduce CPU load, and the value should converge to a constant value
if (iteration < 100) {
dT_filtered = dT;
} else if (iteration < 2000) {
dT_filtered = 0.01f * dT + (1.0f - 0.01f) * dT_filtered;
} else if (iteration == 2000) {
gyro_filter_updated = true;
}
if (gyro_filter_updated) {
if (settings.GyroCutoff < 1.0f) {
gyro_alpha = 0;
} else {
gyro_alpha = expf(-2.0f * (float)(M_PI) *
settings.GyroCutoff * dT_filtered);
}
// Compute time constant for vbar decay term
if (settings.VbarTau < 0.001f) {
vbar_decay = 0;
} else {
vbar_decay = expf(-dT_filtered / settings.VbarTau);
}
gyro_filter_updated = false;
}
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);
AttitudeActualGet(&attitudeActual);
GyrosGet(&gyrosData);
ActuatorDesiredGet(&actuatorDesired);
#if defined(RATEDESIRED_DIAGNOSTICS)
RateDesiredGet(&rateDesired);
#endif
struct TrimmedAttitudeSetpoint {
float Roll;
float Pitch;
float Yaw;
} trimmedAttitudeSetpoint;
// Mux in level trim values, and saturate the trimmed attitude setpoint.
trimmedAttitudeSetpoint.Roll = bound_min_max(
stabDesired.Roll + trimAngles.Roll,
-settings.RollMax + trimAngles.Roll,
settings.RollMax + trimAngles.Roll);
trimmedAttitudeSetpoint.Pitch = bound_min_max(
stabDesired.Pitch + trimAngles.Pitch,
-settings.PitchMax + trimAngles.Pitch,
settings.PitchMax + trimAngles.Pitch);
trimmedAttitudeSetpoint.Yaw = stabDesired.Yaw;
// For horizon mode we need to compute the desire attitude from an unscaled value and apply the
// trim offset. Also track the stick with the most deflection to choose rate blending.
horizonRateFraction = 0.0f;
if (stabDesired.StabilizationMode[ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) {
trimmedAttitudeSetpoint.Roll = bound_min_max(
stabDesired.Roll * settings.RollMax + trimAngles.Roll,
-settings.RollMax + trimAngles.Roll,
settings.RollMax + trimAngles.Roll);
horizonRateFraction = fabsf(stabDesired.Roll);
}
if (stabDesired.StabilizationMode[PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) {
trimmedAttitudeSetpoint.Pitch = bound_min_max(
stabDesired.Pitch * settings.PitchMax + trimAngles.Pitch,
-settings.PitchMax + trimAngles.Pitch,
settings.PitchMax + trimAngles.Pitch);
horizonRateFraction = MAX(horizonRateFraction, fabsf(stabDesired.Pitch));
}
if (stabDesired.StabilizationMode[YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) {
trimmedAttitudeSetpoint.Yaw = stabDesired.Yaw * settings.YawMax;
horizonRateFraction = MAX(horizonRateFraction, fabsf(stabDesired.Yaw));
}
// For weak leveling mode the attitude setpoint is the trim value (drifts back towards "0")
if (stabDesired.StabilizationMode[ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) {
trimmedAttitudeSetpoint.Roll = trimAngles.Roll;
}
if (stabDesired.StabilizationMode[PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) {
trimmedAttitudeSetpoint.Pitch = trimAngles.Pitch;
}
if (stabDesired.StabilizationMode[YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) {
trimmedAttitudeSetpoint.Yaw = 0;
}
// Note we divide by the maximum limit here so the fraction ranges from 0 to 1 depending on
// how much is requested.
horizonRateFraction = bound_sym(horizonRateFraction, HORIZON_MODE_MAX_BLEND) / HORIZON_MODE_MAX_BLEND;
// Calculate the errors in each axis. The local error is used in the following modes:
// ATTITUDE, HORIZON, WEAKLEVELING
float local_attitude_error[3];
local_attitude_error[0] = trimmedAttitudeSetpoint.Roll - attitudeActual.Roll;
local_attitude_error[1] = trimmedAttitudeSetpoint.Pitch - attitudeActual.Pitch;
local_attitude_error[2] = trimmedAttitudeSetpoint.Yaw - attitudeActual.Yaw;
// Wrap yaw error to [-180,180]
local_attitude_error[2] = circular_modulus_deg(local_attitude_error[2]);
static float gyro_filtered[3];
gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha);
gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha);
gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha);
// A flag to track which stabilization mode each axis is in
static uint8_t previous_mode[MAX_AXES] = {255,255,255};
bool error = false;
//Run the selected stabilization algorithm on each axis:
for(uint8_t i=0; i< MAX_AXES; i++)
{
// Check whether this axis mode needs to be reinitialized
bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]);
// The unscaled input (-1,1)
float *raw_input = &stabDesired.Roll;
previous_mode[i] = stabDesired.StabilizationMode[i];
// Apply the selected control law
switch(stabDesired.StabilizationMode[i])
{
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
if(reinit)
pids[PID_GROUP_RATE + i].iAccumulator = 0;
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ACROPLUS:
// this implementation is based on the Openpilot/Librepilot Acro+ flightmode
// and our existing rate & MWRate flightmodes
if(reinit)
pids[PID_GROUP_RATE + i].iAccumulator = 0;
// The factor for gyro suppression / mixing raw stick input into the output; scaled by raw stick input
float factor = fabsf(raw_input[i]) * settings.AcroInsanityFactor / 100;
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound_sym(raw_input[i] * settings.ManualRate[i], settings.ManualRate[i]);
// Zero integral for aggressive maneuvers, like it is done for MWRate
if ((i < 2 && fabsf(gyro_filtered[i]) > 150.0f) ||
(i == 0 && fabsf(raw_input[i]) > 0.2f)) {
pids[PID_GROUP_RATE + i].iAccumulator = 0;
pids[PID_GROUP_RATE + i].i = 0;
}
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = factor * raw_input[i] + (1.0f - factor) * actuatorDesiredAxis[i];
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i], 1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
if(reinit) {
pids[PID_GROUP_ATT + i].iAccumulator = 0;
pids[PID_GROUP_RATE + i].iAccumulator = 0;
}
// Compute the outer loop
rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i], dT);
rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
// Store for debugging output
rateDesiredAxis[i] = stabDesiredAxis[i];
// Run a virtual flybar stabilization algorithm on this axis
stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &pids[PID_GROUP_VBAR + i], &settings);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
{
if (reinit)
pids[PID_GROUP_RATE + i].iAccumulator = 0;
float weak_leveling = local_attitude_error[i] * weak_leveling_kp;
weak_leveling = bound_sym(weak_leveling, weak_leveling_max);
// Compute desired rate as input biased towards leveling
rateDesiredAxis[i] = stabDesiredAxis[i] + weak_leveling;
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
if (reinit)
pids[PID_GROUP_RATE + i].iAccumulator = 0;
if (fabsf(stabDesiredAxis[i]) > max_axislock_rate) {
// While getting strong commands act like rate mode
rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]);
// Reset accumulator
axis_lock_accum[i] = 0;
} else {
// For weaker commands or no command simply lock (almost) on no gyro change
axis_lock_accum[i] += (stabDesiredAxis[i] - gyro_filtered[i]) * dT;
axis_lock_accum[i] = bound_sym(axis_lock_accum[i], max_axis_lock);
// Compute the inner loop
float tmpRateDesired = pid_apply(&pids[PID_GROUP_ATT + i], axis_lock_accum[i], dT);
rateDesiredAxis[i] = bound_sym(tmpRateDesired, settings.MaximumRate[i]);
}
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON:
if(reinit) {
pids[PID_GROUP_RATE + i].iAccumulator = 0;
}
// Do not allow outer loop integral to wind up in this mode since the controller
// is often disengaged.
pids[PID_GROUP_ATT + i].iAccumulator = 0;
// Compute the outer loop for the attitude control
float rateDesiredAttitude = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i], dT);
// Compute the desire rate for a rate control
float rateDesiredRate = raw_input[i] * settings.ManualRate[i];
// Blend from one rate to another. The maximum of all stick positions is used for the
// amount so that when one axis goes completely to rate the other one does too. This
// prevents doing flips while one axis tries to stay in attitude mode.
rateDesiredAxis[i] = rateDesiredAttitude * (1.0f-horizonRateFraction) + rateDesiredRate * horizonRateFraction;
rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.ManualRate[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_MWRATE:
{
if(reinit) {
pids[PID_GROUP_MWR + i].iAccumulator = 0;
}
/*
Conversion from MultiWii PID settings to our units.
Kp = Kp_mw * 4 / 80 / 500
Kd = Kd_mw * looptime * 1e-6 * 4 * 3 / 32 / 500
Ki = Ki_mw * 4 / 125 / 64 / (looptime * 1e-6) / 500
These values will just be approximate and should help
you get started.
*/
// The unscaled input (-1,1) - note in MW this is from (-500,500)
float *raw_input = &stabDesired.Roll;
// dynamic PIDs are scaled both by throttle and stick position
float scale = (i == 0 || i == 1) ? mwrate_settings.RollPitchRate : mwrate_settings.YawRate;
float pid_scale = (100.0f - scale * fabsf(raw_input[i])) / 100.0f;
float dynP8 = pids[PID_GROUP_MWR + i].p * pid_scale;
float dynD8 = pids[PID_GROUP_MWR + i].d * pid_scale;
// these terms are used by the integral loop this proportional term is scaled by throttle (this is different than MW
// that does not apply scale
float cfgP8 = pids[PID_GROUP_MWR + i].p;
float cfgI8 = pids[PID_GROUP_MWR + i].i;
// Dynamically adjust PID settings
struct pid mw_pid;
mw_pid.p = 0; // use zero Kp here because of strange setpoint. applied later.
mw_pid.d = dynD8;
mw_pid.i = cfgI8;
mw_pid.iLim = pids[PID_GROUP_MWR + i].iLim;
mw_pid.iAccumulator = pids[PID_GROUP_MWR + i].iAccumulator;
mw_pid.lastErr = pids[PID_GROUP_MWR + i].lastErr;
mw_pid.lastDer = pids[PID_GROUP_MWR + i].lastDer;
// Zero integral for aggressive maneuvers
if ((i < 2 && fabsf(gyro_filtered[i]) > 150.0f) ||
(i == 0 && fabsf(raw_input[i]) > 0.2f)) {
mw_pid.iAccumulator = 0;
mw_pid.i = 0;
}
// Apply controller as if we want zero change, then add stick input afterwards
actuatorDesiredAxis[i] = pid_apply_setpoint(&mw_pid, raw_input[i] / cfgP8, gyro_filtered[i], dT);
actuatorDesiredAxis[i] += raw_input[i]; // apply input
actuatorDesiredAxis[i] -= dynP8 * gyro_filtered[i]; // apply Kp term
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
// Store PID accumulators for next cycle
pids[PID_GROUP_MWR + i].iAccumulator = mw_pid.iAccumulator;
pids[PID_GROUP_MWR + i].lastErr = mw_pid.lastErr;
pids[PID_GROUP_MWR + i].lastDer = mw_pid.lastDer;
}
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT:
if(reinit) {
pids[PID_GROUP_ATT + i].iAccumulator = 0;
pids[PID_GROUP_RATE + i].iAccumulator = 0;
}
static uint32_t ident_iteration = 0;
static float ident_offsets[3] = {0};
if (PIOS_DELAY_DiffuS(system_ident_timeval) / 1000.0f > SYSTEM_IDENT_PERIOD && SystemIdentHandle()) {
ident_iteration++;
system_ident_timeval = PIOS_DELAY_GetRaw();
SystemIdentData systemIdent;
SystemIdentGet(&systemIdent);
const float SCALE_BIAS = 7.1f;
float roll_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_ROLL]);
float pitch_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_PITCH]);
float yaw_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_YAW]);
if (roll_scale > 0.25f)
roll_scale = 0.25f;
if (pitch_scale > 0.25f)
pitch_scale = 0.25f;
if (yaw_scale > 0.25f)
yaw_scale = 0.2f;
switch(ident_iteration & 0x07) {
case 0:
ident_offsets[0] = 0;
ident_offsets[1] = 0;
ident_offsets[2] = yaw_scale;
break;
case 1:
ident_offsets[0] = roll_scale;
ident_offsets[1] = 0;
ident_offsets[2] = 0;
break;
case 2:
ident_offsets[0] = 0;
ident_offsets[1] = 0;
ident_offsets[2] = -yaw_scale;
break;
case 3:
ident_offsets[0] = -roll_scale;
ident_offsets[1] = 0;
ident_offsets[2] = 0;
break;
case 4:
ident_offsets[0] = 0;
ident_offsets[1] = 0;
ident_offsets[2] = yaw_scale;
break;
case 5:
ident_offsets[0] = 0;
ident_offsets[1] = pitch_scale;
ident_offsets[2] = 0;
break;
case 6:
ident_offsets[0] = 0;
ident_offsets[1] = 0;
ident_offsets[2] = -yaw_scale;
break;
case 7:
ident_offsets[0] = 0;
ident_offsets[1] = -pitch_scale;
ident_offsets[2] = 0;
break;
}
}
if (i == ROLL || i == PITCH) {
// Compute the outer loop
rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i], dT);
rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] += ident_offsets[i];
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
} else {
// Get the desired rate. yaw is always in rate mode in system ident.
rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]);
// Compute the inner loop only for yaw
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] += ident_offsets[i];
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
}
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_COORDINATEDFLIGHT:
switch (i) {
case YAW:
if (reinit) {
pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0;
pids[PID_RATE_YAW].iAccumulator = 0;
axis_lock_accum[YAW] = 0;
}
//If we are not in roll attitude mode, trigger an error
if (stabDesired.StabilizationMode[ROLL] != STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
{
error = true;
break ;
}
if (fabsf(stabDesired.Yaw) < COORDINATED_FLIGHT_MAX_YAW_THRESHOLD) { //If yaw is within the deadband...
if (fabsf(stabDesired.Roll) > COORDINATED_FLIGHT_MIN_ROLL_THRESHOLD) { // We're requesting more roll than the threshold
float accelsDataY;
AccelsyGet(&accelsDataY);
//Reset integral if we have changed roll to opposite direction from rudder. This implies that we have changed desired turning direction.
if ((stabDesired.Roll > 0 && actuatorDesiredAxis[YAW] < 0) ||
(stabDesired.Roll < 0 && actuatorDesiredAxis[YAW] > 0)){
pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0;
}
// Coordinate flight can simply be seen as ensuring that there is no lateral acceleration in the
// body frame. As such, we use the (noisy) accelerometer data as our measurement. Ideally, at
// some point in the future we will estimate acceleration and then we can use the estimated value
// instead of the measured value.
float errorSlip = -accelsDataY;
float command = pid_apply(&pids[PID_COORDINATED_FLIGHT_YAW], errorSlip, dT);
actuatorDesiredAxis[YAW] = bound_sym(command ,1.0);
// Reset axis-lock integrals
pids[PID_RATE_YAW].iAccumulator = 0;
axis_lock_accum[YAW] = 0;
} else if (fabsf(stabDesired.Roll) <= COORDINATED_FLIGHT_MIN_ROLL_THRESHOLD) { // We're requesting less roll than the threshold
// Axis lock on no gyro change
axis_lock_accum[YAW] += (0 - gyro_filtered[YAW]) * dT;
rateDesiredAxis[YAW] = pid_apply(&pids[PID_ATT_YAW], axis_lock_accum[YAW], dT);
rateDesiredAxis[YAW] = bound_sym(rateDesiredAxis[YAW], settings.MaximumRate[YAW]);
actuatorDesiredAxis[YAW] = pid_apply_setpoint(&pids[PID_RATE_YAW], rateDesiredAxis[YAW], gyro_filtered[YAW], dT);
actuatorDesiredAxis[YAW] = bound_sym(actuatorDesiredAxis[YAW],1.0f);
// Reset coordinated-flight integral
pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0;
}
} else { //... yaw is outside the deadband. Pass the manual input directly to the actuator.
actuatorDesiredAxis[YAW] = bound_sym(stabDesiredAxis[YAW], 1.0);
// Reset all integrals
pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0;
pids[PID_RATE_YAW].iAccumulator = 0;
axis_lock_accum[YAW] = 0;
}
break;
case ROLL:
case PITCH:
default:
//Coordinated Flight has no effect in these modes. Trigger a configuration error.
error = true;
break;
}
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_POI:
// The sanity check enforces this is only selectable for Yaw
// for a gimbal you can select pitch too.
if(reinit) {
pids[PID_GROUP_ATT + i].iAccumulator = 0;
pids[PID_GROUP_RATE + i].iAccumulator = 0;
}
float error;
float angle;
if (CameraDesiredHandle()) {
switch(i) {
case PITCH:
CameraDesiredDeclinationGet(&angle);
error = circular_modulus_deg(angle - attitudeActual.Pitch);
break;
case ROLL:
{
uint8_t roll_fraction = 0;
#ifdef GIMBAL
if (BrushlessGimbalSettingsHandle()) {
BrushlessGimbalSettingsRollFractionGet(&roll_fraction);
}
#endif /* GIMBAL */
// For ROLL POI mode we track the FC roll angle (scaled) to
// allow keeping some motion
CameraDesiredRollGet(&angle);
angle *= roll_fraction / 100.0f;
error = circular_modulus_deg(angle - attitudeActual.Roll);
}
break;
case YAW:
CameraDesiredBearingGet(&angle);
error = circular_modulus_deg(angle - attitudeActual.Yaw);
break;
default:
error = true;
}
} else
error = true;
// Compute the outer loop
rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], error, dT);
rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.PoiMaximumRate[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
actuatorDesiredAxis[i] = bound_sym(stabDesiredAxis[i],1.0f);
break;
default:
error = true;
break;
}
}
if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE)
stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT);
#if defined(RATEDESIRED_DIAGNOSTICS)
RateDesiredSet(&rateDesired);
#endif
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
actuatorDesired.Throttle = stabDesired.Throttle;
if(flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_MANUAL) {
ActuatorDesiredSet(&actuatorDesired);
} else {
// Force all axes to reinitialize when engaged
for(uint8_t i=0; i< MAX_AXES; i++)
previous_mode[i] = 255;
}
if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
(lowThrottleZeroIntegral && stabDesired.Throttle < 0))
{
// Force all axes to reinitialize when engaged
for(uint8_t i=0; i< MAX_AXES; i++)
previous_mode[i] = 255;
}
// Clear or set alarms. Done like this to prevent toggling each cycle
// and hammering system alarms
if (error)
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_ERROR);
else
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}
