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 "mixersettings.h"
  53 #include "actuatorcommand.h"
  54 #include "camerastabsettings.h"
  55 #include "cameradesired.h"
  56 #include "hwsettings.h"
  57
  58 //
  59 // Configuration
  60 //
  61 #define SAMPLE_PERIOD_MS 10
  62
  63 // Private types
  64
  65 // Private variables
  66 static bool gimbalOutputEnabled = false;
  67
  68 static struct CameraStab_data {
  69     portTickType lastSysTime;
  70     float inputs[CAMERASTABSETTINGS_INPUT_NUMELEM];
  71
  72 #ifdef USE_GIMBAL_LPF
  73     float attitudeFiltered[CAMERASTABSETTINGS_INPUT_NUMELEM];
  74 #endif
  75
  76 #ifdef USE_GIMBAL_FF
  77     float ffLastAttitude[CAMERASTABSETTINGS_INPUT_NUMELEM];
  78     float ffLastAttitudeFiltered[CAMERASTABSETTINGS_INPUT_NUMELEM];
  79     float ffFilterAccumulator[CAMERASTABSETTINGS_INPUT_NUMELEM];
  80 #endif
  81 } *csd;
  82
  83 // Private functions
  84 static void attitudeUpdated(UAVObjEvent *ev);
  85
  86 #ifdef USE_GIMBAL_FF
  87 static void applyFeedForward(uint8_t index, float dT, float *attitude, CameraStabSettingsData *cameraStab);
  88 #endif
  89
  90 // this structure is equivalent to the UAVObjects for one mixer.
  91 typedef struct {
  92     uint8_t type;
  93     int8_t  matrix[5];
  94 } __attribute__((packed)) Mixer_t;
  95
  96
  97 /**
  98  * Initialise the module, called on startup
  99  * \returns 0 on success or -1 if initialisation failed
 100  */
 101 int32_t CameraStabInitialize(void)
 102 {
 103     bool cameraStabEnabled;
 104
 105 #ifdef MODULE_CAMERASTAB_BUILTIN
 106     cameraStabEnabled = true;
 107 #else
 108     HwSettingsOptionalModulesData optionalModules;
 109
 110     HwSettingsOptionalModulesGet(&optionalModules);
 111
 112     if (optionalModules.CameraStab == HWSETTINGS_OPTIONALMODULES_ENABLED) {
 113         cameraStabEnabled = true;
 114     } else {
 115         cameraStabEnabled = false;
 116     }
 117 #endif
 118
 119     if (cameraStabEnabled) {
 120         // allocate and initialize the static data storage only if module is enabled
 121         csd = (struct CameraStab_data *)pios_malloc(sizeof(struct CameraStab_data));
 122         if (!csd) {
 123             return -1;
 124         }
 125
 126         // initialize camera state variables
 127         memset(csd, 0, sizeof(struct CameraStab_data));
 128         csd->lastSysTime = xTaskGetTickCount();
 129
 130         AttitudeStateInitialize();
 131         CameraDesiredInitialize();
 132
 133         UAVObjEvent ev = {
 134             .obj    = AttitudeStateHandle(),
 135             .instId = 0,
 136             .event  = 0,
 137             .lowPriority = false,
 138         };
 139         EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
 140
 141         return 0;
 142     }
 143
 144     return -1;
 145 }
 146
 147 /* stub: module has no module thread */
 148 int32_t CameraStabStart(void)
 149 {
 150     return 0;
 151 }
 152
 153 MODULE_INITCALL(CameraStabInitialize, CameraStabStart);
 154
 155 static void attitudeUpdated(UAVObjEvent *ev)
 156 {
 157     if (ev->obj != AttitudeStateHandle()) {
 158         return;
 159     }
 160
 161     if (!gimbalOutputEnabled) {
 162         MixerSettingsData mixerSettings;
 163         MixerSettingsGet(&mixerSettings);
 164         Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type;
 165         for (int ct = 0; ct < ACTUATORCOMMAND_CHANNEL_NUMELEM; ct++) {
 166             uint8_t mixer_type = mixers[ct].type;
 167             if ((mixer_type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) &&
 168                 (mixer_type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) {
 169                 gimbalOutputEnabled = true;
 170             }
 171         }
 172         return;
 173     }
 174
 175     AccessoryDesiredData accessory;
 176
 177     CameraStabSettingsData cameraStab;
 178     CameraStabSettingsGet(&cameraStab);
 179
 180     // check how long since last update, time delta between calls in ms
 181     portTickType thisSysTime = xTaskGetTickCount();
 182     float dT_millis = (thisSysTime > csd->lastSysTime) ?
 183                       (float)((thisSysTime - csd->lastSysTime) * portTICK_RATE_MS) :
 184                       (float)SAMPLE_PERIOD_MS;
 185     csd->lastSysTime = thisSysTime;
 186
 187     // storage for elevon roll component before the pitch component has been generated
 188     // we are guaranteed that the iteration order of i is roll pitch yaw
 189     // that guarnteees this won't be used uninited, but the compiler doesn't know that
 190     // so we init it or turn the warning/error off for each compiler
 191     float elevon_roll = 0.0f;
 192
 193     // process axes
 194     for (uint8_t i = 0; i < CAMERASTABSETTINGS_INPUT_NUMELEM; i++) {
 195         // read and process control input
 196         if (CameraStabSettingsInputToArray(cameraStab.Input)[i] != CAMERASTABSETTINGS_INPUT_NONE) {
 197             if (AccessoryDesiredInstGet(CameraStabSettingsInputToArray(cameraStab.Input)[i] -
 198                                         CAMERASTABSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) {
 199                 float input_rate;
 200                 switch (CameraStabSettingsStabilizationModeToArray(cameraStab.StabilizationMode)[i]) {
 201                 case CAMERASTABSETTINGS_STABILIZATIONMODE_ATTITUDE:
 202                     csd->inputs[i] = accessory.AccessoryVal *
 203                                      CameraStabSettingsInputRangeToArray(cameraStab.InputRange)[i];
 204                     break;
 205                 case CAMERASTABSETTINGS_STABILIZATIONMODE_AXISLOCK:
 206                     input_rate = accessory.AccessoryVal *
 207                                  CameraStabSettingsInputRateToArray(cameraStab.InputRate)[i];
 208                     if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) {
 209                         csd->inputs[i] = boundf(csd->inputs[i] + input_rate * 0.001f * dT_millis,
 210                                                 -CameraStabSettingsInputRangeToArray(cameraStab.InputRange)[i],
 211                                                 CameraStabSettingsInputRangeToArray(cameraStab.InputRange)[i]);
 212                     }
 213                     break;
 214                 default:
 215                     PIOS_Assert(0);
 216                 }
 217             }
 218         }
 219
 220         // calculate servo output
 221         float attitude;
 222
 223         switch (i) {
 224         case CAMERASTABSETTINGS_INPUT_ROLL:
 225             AttitudeStateRollGet(&attitude);
 226             break;
 227         case CAMERASTABSETTINGS_INPUT_PITCH:
 228             AttitudeStatePitchGet(&attitude);
 229             break;
 230         case CAMERASTABSETTINGS_INPUT_YAW:
 231             AttitudeStateYawGet(&attitude);
 232             break;
 233         default:
 234             PIOS_Assert(0);
 235         }
 236
 237 #ifdef USE_GIMBAL_LPF
 238         if (CameraStabSettingsResponseTimeToArray(cameraStab.ResponseTime)[i]) {
 239             float rt = (float)CameraStabSettingsResponseTimeToArray(cameraStab.ResponseTime)[i];
 240             attitude = csd->attitudeFiltered[i] = ((rt * csd->attitudeFiltered[i]) + (dT_millis * attitude)) / (rt + dT_millis);
 241         }
 242 #endif
 243
 244 #ifdef USE_GIMBAL_FF
 245         if (CameraStabSettingsFeedForwardToArray(cameraStab.FeedForward)[i]) {
 246             applyFeedForward(i, dT_millis, &attitude, &cameraStab);
 247         }
 248 #endif
 249
 250         // bounding for elevon mixing occurs on the unmixed output
 251         // to limit the range of the mixed output you must limit the range
 252         // of both the unmixed pitch and unmixed roll
 253         float output = boundf((attitude + csd->inputs[i]) / CameraStabSettingsOutputRangeToArray(cameraStab.OutputRange)[i], -1.0f, 1.0f);
 254
 255         // set output channels
 256         switch (i) {
 257         case CAMERASTABSETTINGS_INPUT_ROLL:
 258             // we are guaranteed that the iteration order of i is roll pitch yaw
 259             // for elevon mixing we simply grab the value for later use
 260             if (cameraStab.GimbalType == CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED) {
 261                 elevon_roll = output;
 262             } else {
 263                 CameraDesiredRollOrServo1Set(&output);
 264             }
 265             break;
 266         case CAMERASTABSETTINGS_INPUT_PITCH:
 267             // we are guaranteed that the iteration order of i is roll pitch yaw
 268             // for elevon mixing we use the value we previously grabbed and set both s1 and s2
 269             if (cameraStab.GimbalType == CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED) {
 270                 float elevon_pitch = output;
 271                 // elevon reversing works like this:
 272                 // first use the normal reversing facilities to get servo 1 roll working in the correct direction
 273                 // then use the normal reversing facilities to get servo 2 roll working in the correct direction
 274                 // then use these new reversing switches to reverse servo 1 and/or 2 pitch as needed
 275                 // if servo 1 pitch is reversed
 276                 if (cameraStab.Servo1PitchReverse == CAMERASTABSETTINGS_SERVO1PITCHREVERSE_TRUE) {
 277                     // use (reversed pitch) + roll
 278                     output = ((1.0f - elevon_pitch) + elevon_roll) / 2.0f;
 279                 } else {
 280                     // use pitch + roll
 281                     output = (elevon_pitch + elevon_roll) / 2.0f;
 282                 }
 283                 CameraDesiredRollOrServo1Set(&output);
 284                 // if servo 2 pitch is reversed
 285                 if (cameraStab.Servo2PitchReverse == CAMERASTABSETTINGS_SERVO2PITCHREVERSE_TRUE) {
 286                     // use (reversed pitch) - roll
 287                     output = ((1.0f - elevon_pitch) - elevon_roll) / 2.0f;
 288                 } else {
 289                     // use pitch - roll
 290                     output = (elevon_pitch - elevon_roll) / 2.0f;
 291                 }
 292                 CameraDesiredPitchOrServo2Set(&output);
 293             } else {
 294                 CameraDesiredPitchOrServo2Set(&output);
 295             }
 296             break;
 297         case CAMERASTABSETTINGS_INPUT_YAW:
 298             CameraDesiredYawSet(&output);
 299             break;
 300         default:
 301             PIOS_Assert(0);
 302         }
 303     }
 304 }
 305
 306 #ifdef USE_GIMBAL_FF
 307 void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraStabSettingsData *cameraStab)
 308 {
 309     // compensate high feed forward values depending on gimbal type
 310     float gimbalTypeCorrection = 1.0f;
 311
 312     switch (cameraStab->GimbalType) {
 313     case CAMERASTABSETTINGS_GIMBALTYPE_GENERIC:
 314     case CAMERASTABSETTINGS_GIMBALTYPE_ROLLPITCHMIXED:
 315         // no correction
 316         break;
 317     case CAMERASTABSETTINGS_GIMBALTYPE_YAWROLLPITCH:
 318         if (index == CAMERASTABSETTINGS_INPUT_ROLL) {
 319             float pitch;
 320             AttitudeStatePitchGet(&pitch);
 321             gimbalTypeCorrection = (cameraStab->OutputRange.Pitch - fabsf(pitch))
 322                                    / cameraStab->OutputRange.Pitch;
 323         }
 324         break;
 325     case CAMERASTABSETTINGS_GIMBALTYPE_YAWPITCHROLL:
 326         if (index == CAMERASTABSETTINGS_INPUT_PITCH) {
 327             float roll;
 328             AttitudeStateRollGet(&roll);
 329             gimbalTypeCorrection = (cameraStab->OutputRange.Roll - fabsf(roll))
 330                                    / cameraStab->OutputRange.Roll;
 331         }
 332         break;
 333     default:
 334         PIOS_Assert(0);
 335     }
 336
 337     // apply feed forward
 338     float accumulator = csd->ffFilterAccumulator[index];
 339     accumulator += (*attitude - csd->ffLastAttitude[index]) *
 340                    (float)CameraStabSettingsFeedForwardToArray(cameraStab->FeedForward)[index] * gimbalTypeCorrection;
 341     csd->ffLastAttitude[index] = *attitude;
 342     *attitude   += accumulator;
 343
 344     float filter = (float)((accumulator > 0.0f) ? CameraStabSettingsAccelTimeToArray(cameraStab->AccelTime)[index] :
 345                            CameraStabSettingsDecelTimeToArray(cameraStab->DecelTime)[index]) / dT_millis;
 346     if (filter < 1.0f) {
 347         filter = 1.0f;
 348     }
 349     accumulator -= accumulator / filter;
 350     csd->ffFilterAccumulator[index] = accumulator;
 351     *attitude   += accumulator;
 352
 353     // apply acceleration limit
 354     float delta    = *attitude - csd->ffLastAttitudeFiltered[index];
 355     float maxDelta = (float)cameraStab->MaxAccel * 0.001f * dT_millis;
 356
 357     if (fabsf(delta) > maxDelta) {
 358         // we are accelerating too hard
 359         *attitude = csd->ffLastAttitudeFiltered[index] + ((delta > 0.0f) ? maxDelta : -maxDelta);
 360     }
 361     csd->ffLastAttitudeFiltered[index] = *attitude;
 362 }
 363 #endif // USE_GIMBAL_FF
 364
 365 /**
 366  * @}
 367  */
 368
 369 /**
 370  * @}
 371  */