flight/modules/CameraStab/camerastab.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup CameraStab Camera Stabilization Module
   6  * @brief Camera stabilization module
   7  * Updates accessory outputs with values appropriate for camera stabilization
   8  * @{
   9  *
  10  * @file       camerastab.c
  11  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  12  * @brief      Stabilize camera against the roll pitch and yaw of aircraft
  13  *
  14  * @see        The GNU Public License (GPL) Version 3
  15  *
  16  *****************************************************************************/
  17 /*
  18  * This program is free software; you can redistribute it and/or modify
  19  * it under the terms of the GNU General Public License as published by
  20  * the Free Software Foundation; either version 3 of the License, or
  21  * (at your option) any later version.
  22  *
  23  * This program is distributed in the hope that it will be useful, but
  24  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  25  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  26  * for more details.
  27  *
  28  * You should have received a copy of the GNU General Public License along
  29  * with this program; if not, write to the Free Software Foundation, Inc.,
  30  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  31  */
  32
  33 /**
  34  * Output object: Accessory
  35  *
  36  * This module will periodically calculate the output values for stabilizing the camera
  37  *
  38  * UAVObjects are automatically generated by the UAVObjectGenerator from
  39  * the object definition XML file.
  40  *
  41  * Modules have no API, all communication to other modules is done through UAVObjects.
  42  * However modules may use the API exposed by shared libraries.
  43  * See the OpenPilot wiki for more details.
  44  * http://www.openpilot.org/OpenPilot_Application_Architecture
  45  *
  46  */
  47
  48 #include "openpilot.h"
  49
  50 #include "accessorydesired.h"
  51 #include "attitudestate.h"
  52 #include "camerastabsettings.h"
  53 #include "cameradesired.h"
  54 #include "hwsettings.h"
  55
  56 //
  57 // Configuration
  58 //
  59 #define SAMPLE_PERIOD_MS 10
  60
  61 // Private types
  62
  63 // Private variables
  64 static struct CameraStab_data {
  65     portTickType lastSysTime;
  66     float inputs[CAMERASTABSETTINGS_INPUT_NUMELEM];
  67
  68 #ifdef USE_GIMBAL_LPF
  69     float attitudeFiltered[CAMERASTABSETTINGS_INPUT_NUMELEM];
  70 #endif
  71
  72 #ifdef USE_GIMBAL_FF
  73     float ffLastAttitude[CAMERASTABSETTINGS_INPUT_NUMELEM];
  74     float ffLastAttitudeFiltered[CAMERASTABSETTINGS_INPUT_NUMELEM];
  75     float ffFilterAccumulator[CAMERASTABSETTINGS_INPUT_NUMELEM];
  76 #endif
  77 } *csd;
  78
  79 // Private functions
  80 static void attitudeUpdated(UAVObjEvent *ev);
  81
  82 #ifdef USE_GIMBAL_FF
  83 static void applyFeedForward(uint8_t index, float dT, float *attitude, CameraStabSettingsData *cameraStab);
  84 #endif
  85
  86
  87 /**
  88  * Initialise the module, called on startup
  89  * \returns 0 on success or -1 if initialisation failed
  90  */
  91 int32_t CameraStabInitialize(void)
  92 {
  93     bool cameraStabEnabled;
  94
  95 #ifdef MODULE_CAMERASTAB_BUILTIN
  96     cameraStabEnabled = true;
  97 #else
  98     HwSettingsOptionalModulesData optionalModules;
  99
 100     HwSettingsInitialize();
 101     HwSettingsOptionalModulesGet(&optionalModules);
 102
 103     if (optionalModules.CameraStab == HWSETTINGS_OPTIONALMODULES_ENABLED) {
 104         cameraStabEnabled = true;
 105     } else {
 106         cameraStabEnabled = false;
 107     }
 108 #endif
 109
 110     if (cameraStabEnabled) {
 111         // allocate and initialize the static data storage only if module is enabled
 112         csd = (struct CameraStab_data *)pios_malloc(sizeof(struct CameraStab_data));
 113         if (!csd) {
 114             return -1;
 115         }
 116
 117         // initialize camera state variables
 118         memset(csd, 0, sizeof(struct CameraStab_data));
 119         csd->lastSysTime = xTaskGetTickCount();
 120
 121         AttitudeStateInitialize();
 122         CameraStabSettingsInitialize();
 123         CameraDesiredInitialize();
 124
 125         UAVObjEvent ev = {
 126             .obj    = AttitudeStateHandle(),
 127             .instId = 0,
 128             .event  = 0,
 129             .lowPriority = false,
 130         };
 131         EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
 132
 133         return 0;
 134     }
 135
 136     return -1;
 137 }
 138
 139 /* stub: module has no module thread */
 140 int32_t CameraStabStart(void)
 141 {
 142     return 0;
 143 }
 144
 145 MODULE_INITCALL(CameraStabInitialize, CameraStabStart);
 146
 147 static void attitudeUpdated(UAVObjEvent *ev)
 148 {
 149     if (ev->obj != AttitudeStateHandle()) {
 150         return;
 151     }
 152
 153     AccessoryDesiredData accessory;
 154
 155     CameraStabSettingsData cameraStab;
 156     CameraStabSettingsGet(&cameraStab);
 157
 158     // check how long since last update, time delta between calls in ms
 159     portTickType thisSysTime = xTaskGetTickCount();
 160     float dT_millis = (thisSysTime > csd->lastSysTime) ?
 161                       (float)((thisSysTime - csd->lastSysTime) * portTICK_RATE_MS) :
 162                       (float)SAMPLE_PERIOD_MS;
 163     csd->lastSysTime = thisSysTime;
 164
 165     // storage for elevon roll component before the pitch component has been generated
 166     // we are guaranteed that the iteration order of i is roll pitch yaw
 167     // that guarnteees this won't be used uninited, but the compiler doesn't know that
 168     // so we init it or turn the warning/error off for each compiler
 169     float elevon_roll = 0.0f;
 170
 171     // process axes
 172     for (uint8_t i = 0; i < CAMERASTABSETTINGS_INPUT_NUMELEM; i++) {
 173         // read and process control input
 174         if (CameraStabSettingsInputToArray(cameraStab.Input)[i] != CAMERASTABSETTINGS_INPUT_NONE) {
 175             if (AccessoryDesiredInstGet(CameraStabSettingsInputToArray(cameraStab.Input)[i] -
 176                                         CAMERASTABSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) {
 177                 float input_rate;
 178                 switch (CameraStabSettingsStabilizationModeToArray(cameraStab.StabilizationMode)[i]) {
 179                 case CAMERASTABSETTINGS_STABILIZATIONMODE_ATTITUDE:
 180                     csd->inputs[i] = accessory.AccessoryVal *
 181                                      CameraStabSettingsInputRangeToArray(cameraStab.InputRange)[i];
 182                     break;
 183                 case CAMERASTABSETTINGS_STABILIZATIONMODE_AXISLOCK:
 184                     input_rate = accessory.AccessoryVal *
 185                                  CameraStabSettingsInputRateToArray(cameraStab.InputRate)[i];
 186                     if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) {
 187                         csd->inputs[i] = boundf(csd->inputs[i] + input_rate * 0.001f * dT_millis,
 188                                                 -CameraStabSettingsInputRangeToArray(cameraStab.InputRange)[i],
 189                                                 CameraStabSettingsInputRangeToArray(cameraStab.InputRange)[i]);
 190                     }
 191                     break;
 192                 default:
 193                     PIOS_Assert(0);
 194                 }
 195             }
 196         }
 197
 198         // calculate servo output
 199         float attitude;
 200
 201         switch (i) {
 202         case CAMERASTABSETTINGS_INPUT_ROLL:
 203             AttitudeStateRollGet(&attitude);
 204             break;
 205         case CAMERASTABSETTINGS_INPUT_PITCH:
 206             AttitudeStatePitchGet(&attitude);
 207             break;
 208         case CAMERASTABSETTINGS_INPUT_YAW:
 209             AttitudeStateYawGet(&attitude);
 210             break;
 211         default:
 212             PIOS_Assert(0);
 213         }
 214
 215 #ifdef USE_GIMBAL_LPF
 216         if (CameraStabSettingsResponseTimeToArray(cameraStab.ResponseTime)[i]) {
 217             float rt = (float)CameraStabSettingsResponseTimeToArray(cameraStab.ResponseTime)[i];
 218             attitude = csd->attitudeFiltered[i] = ((rt * csd->attitudeFiltered[i]) + (dT_millis * attitude)) / (rt + dT_millis);
 219         }
 220 #endif
 221
 222 #ifdef USE_GIMBAL_FF
 223         if (CameraStabSettingsFeedForwardToArray(cameraStab.FeedForward)[i]) {
 224             applyFeedForward(i, dT_millis, &attitude, &cameraStab);
 225         }
 226 #endif
 227
 228         // bounding for elevon mixing occurs on the unmixed output
 229         // to limit the range of the mixed output you must limit the range
 230         // of both the unmixed pitch and unmixed roll
 231         float output = boundf((attitude + csd->inputs[i]) / CameraStabSettingsOutputRangeToArray(cameraStab.OutputRange)[i], -1.0f, 1.0f);
 232
 233         // set output channels
 234         switch (i) {
 235         case CAMERASTABSETTINGS_INPUT_ROLL:
 236             // we are guaranteed that the iteration order of i is roll pitch yaw
 237             // for elevon mixing we simply grab the value for later use
 238             if (cameraStab.GimbalType == CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED) {
 239                 elevon_roll = output;
 240             } else {
 241                 CameraDesiredRollOrServo1Set(&output);
 242             }
 243             break;
 244         case CAMERASTABSETTINGS_INPUT_PITCH:
 245             // we are guaranteed that the iteration order of i is roll pitch yaw
 246             // for elevon mixing we use the value we previously grabbed and set both s1 and s2
 247             if (cameraStab.GimbalType == CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED) {
 248                 float elevon_pitch = output;
 249                 // elevon reversing works like this:
 250                 // first use the normal reversing facilities to get servo 1 roll working in the correct direction
 251                 // then use the normal reversing facilities to get servo 2 roll working in the correct direction
 252                 // then use these new reversing switches to reverse servo 1 and/or 2 pitch as needed
 253                 // if servo 1 pitch is reversed
 254                 if (cameraStab.Servo1PitchReverse == CAMERASTABSETTINGS_SERVO1PITCHREVERSE_TRUE) {
 255                     // use (reversed pitch) + roll
 256                     output = ((1.0f - elevon_pitch) + elevon_roll) / 2.0f;
 257                 } else {
 258                     // use pitch + roll
 259                     output = (elevon_pitch + elevon_roll) / 2.0f;
 260                 }
 261                 CameraDesiredRollOrServo1Set(&output);
 262                 // if servo 2 pitch is reversed
 263                 if (cameraStab.Servo2PitchReverse == CAMERASTABSETTINGS_SERVO2PITCHREVERSE_TRUE) {
 264                     // use (reversed pitch) - roll
 265                     output = ((1.0f - elevon_pitch) - elevon_roll) / 2.0f;
 266                 } else {
 267                     // use pitch - roll
 268                     output = (elevon_pitch - elevon_roll) / 2.0f;
 269                 }
 270                 CameraDesiredPitchOrServo2Set(&output);
 271             } else {
 272                 CameraDesiredPitchOrServo2Set(&output);
 273             }
 274             break;
 275         case CAMERASTABSETTINGS_INPUT_YAW:
 276             CameraDesiredYawSet(&output);
 277             break;
 278         default:
 279             PIOS_Assert(0);
 280         }
 281     }
 282 }
 283
 284 #ifdef USE_GIMBAL_FF
 285 void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraStabSettingsData *cameraStab)
 286 {
 287     // compensate high feed forward values depending on gimbal type
 288     float gimbalTypeCorrection = 1.0f;
 289
 290     switch (cameraStab->GimbalType) {
 291     case CAMERASTABSETTINGS_GIMBALTYPE_GENERIC:
 292     case CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED:
 293         // no correction
 294         break;
 295     case CAMERASTABSETTINGS_GIMBALTYPE_YAWROLLPITCH:
 296         if (index == CAMERASTABSETTINGS_INPUT_ROLL) {
 297             float pitch;
 298             AttitudeStatePitchGet(&pitch);
 299             gimbalTypeCorrection = (cameraStab->OutputRange.Pitch - fabsf(pitch))
 300                                    / cameraStab->OutputRange.Pitch;
 301         }
 302         break;
 303     case CAMERASTABSETTINGS_GIMBALTYPE_YAWPITCHROLL:
 304         if (index == CAMERASTABSETTINGS_INPUT_PITCH) {
 305             float roll;
 306             AttitudeStateRollGet(&roll);
 307             gimbalTypeCorrection = (cameraStab->OutputRange.Roll - fabsf(roll))
 308                                    / cameraStab->OutputRange.Roll;
 309         }
 310         break;
 311     default:
 312         PIOS_Assert(0);
 313     }
 314
 315     // apply feed forward
 316     float accumulator = csd->ffFilterAccumulator[index];
 317     accumulator += (*attitude - csd->ffLastAttitude[index]) *
 318                    (float)CameraStabSettingsFeedForwardToArray(cameraStab->FeedForward)[index] * gimbalTypeCorrection;
 319     csd->ffLastAttitude[index] = *attitude;
 320     *attitude   += accumulator;
 321
 322     float filter = (float)((accumulator > 0.0f) ? CameraStabSettingsAccelTimeToArray(cameraStab->AccelTime)[index] :
 323                            CameraStabSettingsDecelTimeToArray(cameraStab->DecelTime)[index]) / dT_millis;
 324     if (filter < 1.0f) {
 325         filter = 1.0f;
 326     }
 327     accumulator -= accumulator / filter;
 328     csd->ffFilterAccumulator[index] = accumulator;
 329     *attitude   += accumulator;
 330
 331     // apply acceleration limit
 332     float delta    = *attitude - csd->ffLastAttitudeFiltered[index];
 333     float maxDelta = (float)cameraStab->MaxAccel * 0.001f * dT_millis;
 334
 335     if (fabsf(delta) > maxDelta) {
 336         // we are accelerating too hard
 337         *attitude = csd->ffLastAttitudeFiltered[index] + ((delta > 0.0f) ? maxDelta : -maxDelta);
 338     }
 339     csd->ffLastAttitudeFiltered[index] = *attitude;
 340 }
 341 #endif // USE_GIMBAL_FF
 342
 343 /**
 344  * @}
 345  */
 346
 347 /**
 348  * @}
 349  */