/** * @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); } }