flight/modules/CameraControl/cameracontrol.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       cameracontrol.c
   5  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
   6  * @brief      camera control module. triggers cameras with multiple options
   7  *
   8  * @see        The GNU Public License (GPL) Version 3
   9  *
  10  *****************************************************************************/
  11 /*
  12  * This program is free software; you can redistribute it and/or modify
  13  * it under the terms of the GNU General Public License as published by
  14  * the Free Software Foundation; either version 3 of the License, or
  15  * (at your option) any later version.
  16  *
  17  * This program is distributed in the hope that it will be useful, but
  18  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  19  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  20  * for more details.
  21  *
  22  * You should have received a copy of the GNU General Public License along
  23  * with this program; if not, write to the Free Software Foundation, Inc.,
  24  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  25  *
  26  * Additional note on redistribution: The copyright and license notices above
  27  * must be maintained in each individual source file that is a derivative work
  28  * of this source file; otherwise redistribution is prohibited.
  29  */
  30
  31 #include <openpilot.h>
  32 #include "inc/cameracontrol.h"
  33 #include <CoordinateConversions.h>
  34 #include <cameradesired.h>
  35 #include <cameracontrolsettings.h>
  36 #include <cameracontrolactivity.h>
  37 #include <accessorydesired.h>
  38 #include <attitudestate.h>
  39 #include <callbackinfo.h>
  40 #include <flightstatus.h>
  41 #include <gpstime.h>
  42 #include <homelocation.h>
  43 #include <hwsettings.h>
  44 #include <positionstate.h>
  45 #include <velocitystate.h>
  46
  47 // Private variables
  48
  49 typedef enum {
  50     CAMERASTATUS_Idle = 0,
  51     CAMERASTATUS_Shot,
  52     CAMERASTATUS_Video
  53 } CameraStatus;
  54
  55 static struct CameraControl_data {
  56     int32_t lastTriggerTimeRaw;
  57     float   lastTriggerNEDPosition[3];
  58     CameraControlSettingsData settings;
  59     CameraControlActivityData activity;
  60     DelayedCallbackInfo *callbackHandle;
  61     CameraStatus outputStatus;
  62     CameraStatus lastOutputStatus;
  63     CameraStatus manualInput;
  64     CameraStatus lastManualInput;
  65     bool autoTriggerEnabled;
  66     uint16_t     ImageId;
  67     float HomeECEF[3];
  68     float HomeRne[3][3];
  69 } *ccd;
  70
  71 #define CALLBACK_PRIORITY   CALLBACK_PRIORITY_REGULAR
  72 #define CBTASK_PRIORITY     CALLBACK_TASK_AUXILIARY
  73 #define STACK_SIZE_BYTES    512
  74 #define CALLBACK_STD_PERIOD 50
  75 #define INPUT_DEADBAND      0.1f
  76
  77 static void CameraControlTask();
  78 static void SettingsUpdateCb(__attribute__((unused)) UAVObjEvent *ev);
  79 static void HomeLocationUpdateCb(__attribute__((unused)) UAVObjEvent *ev);
  80 static void UpdateOutput();
  81 static void PublishActivity();
  82 static bool checkActivation();
  83 static void FillActivityInfo();
  84
  85
  86 /**
  87  * Initialise the module, called on startup
  88  * \returns 0 on success or -1 if initialisation failed
  89  */
  90 int32_t CameraControlInitialize(void)
  91 {
  92     ccd = 0;
  93     HwSettingsInitialize();
  94     HwSettingsOptionalModulesData modules;
  95     HwSettingsOptionalModulesGet(&modules);
  96     if (modules.CameraControl == HWSETTINGS_OPTIONALMODULES_ENABLED) {
  97         ccd = (struct CameraControl_data *)pios_malloc(sizeof(struct CameraControl_data));
  98         memset(ccd, 0, sizeof(struct CameraControl_data));
  99         ccd->callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&CameraControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_CAMERACONTROL, STACK_SIZE_BYTES);
 100         CameraControlActivityInitialize();
 101         CameraDesiredInitialize();
 102         CameraControlSettingsInitialize();
 103         CameraControlSettingsConnectCallback(SettingsUpdateCb);
 104         HomeLocationInitialize();
 105         HomeLocationConnectCallback(HomeLocationUpdateCb);
 106         GPSTimeInitialize();
 107         PositionStateInitialize();
 108         AttitudeStateInitialize();
 109         AccessoryDesiredInitialize();
 110         FlightStatusInitialize();
 111
 112
 113         SettingsUpdateCb(NULL);
 114         HomeLocationUpdateCb(NULL);
 115
 116         // init output:
 117         ccd->outputStatus = CAMERASTATUS_Idle;
 118         UpdateOutput();
 119     }
 120     return 0;
 121 }
 122
 123 /* stub: module has no module thread */
 124 int32_t CameraControlStart(void)
 125 {
 126     if (!ccd) {
 127         return 0;
 128     }
 129
 130     PIOS_CALLBACKSCHEDULER_Schedule(ccd->callbackHandle, CALLBACK_STD_PERIOD, CALLBACK_UPDATEMODE_LATER);
 131     ccd->lastTriggerTimeRaw = PIOS_DELAY_GetRaw();
 132     return 0;
 133 }
 134
 135 MODULE_INITCALL(CameraControlInitialize, CameraControlStart);
 136
 137 static void CameraControlTask()
 138 {
 139     bool trigger = false;
 140     PositionStateData pos;
 141     uint32_t timeSinceLastShot = PIOS_DELAY_DiffuS(ccd->lastTriggerTimeRaw);
 142     CameraStatus newStatus;
 143
 144     if (checkActivation()) {
 145         if (ccd->manualInput != ccd->lastManualInput && ccd->manualInput != CAMERASTATUS_Idle) {
 146             // Manual override
 147             trigger   = true;
 148             newStatus = ccd->manualInput;
 149             ccd->activity.Reason = CAMERACONTROLACTIVITY_REASON_MANUAL;
 150         } else {
 151             // MinimumTimeInterval sets a hard limit on time between two consecutive shots, i.e. the minimum time between shots the camera can achieve
 152             if (ccd->autoTriggerEnabled &&
 153                 timeSinceLastShot > (ccd->settings.MinimumTimeInterval * 1000 * 1000)) {
 154                 // check trigger conditions
 155                 if (ccd->settings.TimeInterval > 0) {
 156                     if (timeSinceLastShot > ccd->settings.TimeInterval * (1000 * 1000)) {
 157                         trigger   = true;
 158                         newStatus = CAMERASTATUS_Shot;
 159                         ccd->activity.Reason = CAMERACONTROLACTIVITY_REASON_AUTOTIME;
 160                     }
 161                 }
 162
 163                 if (ccd->settings.SpaceInterval > 0) {
 164                     PositionStateGet(&pos);
 165                     float dn = pos.North - ccd->lastTriggerNEDPosition[0];
 166                     float de = pos.East - ccd->lastTriggerNEDPosition[1];
 167                     float distance = sqrtf((dn * dn) + (de * de));
 168                     ccd->activity.TriggerMillisecond = (int16_t)distance * 10.0f;
 169                     if (distance > ccd->settings.SpaceInterval) {
 170                         trigger   = true;
 171                         newStatus = CAMERASTATUS_Shot;
 172                         ccd->activity.Reason = CAMERACONTROLACTIVITY_REASON_AUTODISTANCE;
 173                     }
 174                 }
 175             }
 176         }
 177     }
 178     if (trigger) {
 179         ccd->outputStatus = newStatus;
 180         ccd->ImageId++;
 181         ccd->lastTriggerTimeRaw = PIOS_DELAY_GetRaw();
 182         ccd->lastTriggerNEDPosition[0] = pos.North;
 183         ccd->lastTriggerNEDPosition[1] = pos.East;
 184         ccd->lastTriggerNEDPosition[2] = pos.Down;
 185     } else {
 186         ccd->outputStatus = CAMERASTATUS_Idle;
 187         ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_IDLE;
 188     }
 189
 190     ccd->lastManualInput = ccd->manualInput;
 191     PublishActivity();
 192     UpdateOutput();
 193     PIOS_CALLBACKSCHEDULER_Schedule(ccd->callbackHandle, CALLBACK_STD_PERIOD, CALLBACK_UPDATEMODE_SOONER);
 194 }
 195
 196 static void SettingsUpdateCb(__attribute__((unused)) UAVObjEvent *ev)
 197 {
 198     CameraControlSettingsGet(&ccd->settings);
 199 }
 200
 201 static bool checkActivation()
 202 {
 203     if (ccd->settings.ManualTriggerInput != CAMERACONTROLSETTINGS_MANUALTRIGGERINPUT_NONE) {
 204         uint8_t accessory = ccd->settings.ManualTriggerInput - CAMERACONTROLSETTINGS_MANUALTRIGGERINPUT_ACCESSORY0;
 205
 206         AccessoryDesiredData accessoryDesired;
 207         AccessoryDesiredInstGet(accessory, &accessoryDesired);
 208
 209         if (fabsf(accessoryDesired.AccessoryVal - ccd->settings.InputValues.Shot) < INPUT_DEADBAND) {
 210             ccd->manualInput = CAMERASTATUS_Shot;
 211         } else if (fabsf(accessoryDesired.AccessoryVal - ccd->settings.InputValues.Video) < INPUT_DEADBAND) {
 212             ccd->manualInput = CAMERASTATUS_Video;
 213         } else {
 214             ccd->manualInput = CAMERASTATUS_Idle;
 215         }
 216     }
 217
 218     switch (ccd->settings.AutoTriggerMode) {
 219     case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_DISABLED:
 220         ccd->autoTriggerEnabled = false;
 221         break;
 222     case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_WHENARMED:
 223     {
 224         FlightStatusArmedOptions armed;
 225
 226         FlightStatusArmedGet(&armed);
 227         ccd->autoTriggerEnabled = (armed == FLIGHTSTATUS_ARMED_ARMED);
 228     }
 229     break;
 230     case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_ALWAYS:
 231         ccd->autoTriggerEnabled = true;
 232         break;
 233     case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_INPUT:
 234     {
 235         uint8_t accessory = ccd->settings.AutoTriggerInput - CAMERACONTROLSETTINGS_AUTOTRIGGERINPUT_ACCESSORY0;
 236         AccessoryDesiredData accessoryDesired;
 237         AccessoryDesiredInstGet(accessory, &accessoryDesired);
 238
 239         ccd->autoTriggerEnabled = (accessoryDesired.AccessoryVal > INPUT_DEADBAND);
 240     }
 241     break;
 242     case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_MISSION:
 243     {
 244         FlightStatusFlightModeOptions flightmode;
 245         FlightStatusFlightModeGet(&flightmode);
 246         ccd->autoTriggerEnabled = (flightmode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER);
 247     }
 248     break;
 249     }
 250     return ccd->autoTriggerEnabled || (ccd->manualInput != CAMERASTATUS_Idle);
 251 }
 252
 253 static void UpdateOutput()
 254 {
 255     if (ccd->outputStatus != ccd->lastOutputStatus) {
 256         switch (ccd->outputStatus) {
 257         case CAMERASTATUS_Idle:
 258             if (CAMERASTATUS_Shot == ccd->lastOutputStatus) {
 259                 if (PIOS_DELAY_DiffuS(ccd->lastTriggerTimeRaw) > ccd->settings.TriggerPulseWidth * 1000) {
 260                     CameraDesiredTriggerSet(&ccd->settings.OutputValues.Idle);
 261                 } else {
 262                     // skip updating lastOutputStatus until TriggerPulseWidth elapsed
 263                     return;
 264                 }
 265             }
 266             break;
 267         case CAMERASTATUS_Shot:
 268             CameraDesiredTriggerSet(&ccd->settings.OutputValues.Shot);
 269             break;
 270         case CAMERASTATUS_Video:
 271             CameraDesiredTriggerSet(&ccd->settings.OutputValues.Video);
 272             break;
 273         }
 274     }
 275     ccd->lastOutputStatus = ccd->outputStatus;
 276 }
 277
 278 static void PublishActivity()
 279 {
 280     if (ccd->outputStatus != ccd->lastOutputStatus) {
 281         switch (ccd->outputStatus) {
 282         case CAMERASTATUS_Idle:
 283             if (ccd->lastOutputStatus == CAMERASTATUS_Video) {
 284                 ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_STOPVIDEO;
 285             } else {
 286                 ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_IDLE;
 287             }
 288             break;
 289         case CAMERASTATUS_Shot:
 290             ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_TRIGGERPICTURE;
 291             break;
 292         case CAMERASTATUS_Video:
 293             if (ccd->lastOutputStatus != CAMERASTATUS_Video) {
 294                 ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_STARTVIDEO;
 295             } else {
 296                 ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_IDLE;
 297             }
 298             break;
 299         }
 300         if (ccd->activity.Activity != CAMERACONTROLACTIVITY_ACTIVITY_IDLE
 301             || ccd->lastOutputStatus != CAMERASTATUS_Shot) {
 302             FillActivityInfo();
 303             CameraControlActivitySet(&ccd->activity);
 304         }
 305     }
 306 }
 307
 308 static void FillActivityInfo()
 309 {
 310     CameraControlActivityData *activity = &ccd->activity;
 311     {
 312         PositionStateData position;
 313         PositionStateGet(&position);
 314         int32_t LLAi[3];
 315         const float pos[3] = {
 316             position.North,
 317             position.East,
 318             position.Down
 319         };
 320         Base2LLA(pos, ccd->HomeECEF, ccd->HomeRne, LLAi);
 321
 322         activity->Latitude  = LLAi[0];
 323         activity->Longitude = LLAi[1];
 324         activity->Altitude  = ((float)LLAi[2]) * 1e-4f;
 325     }
 326     {
 327         GPSTimeData time;
 328         GPSTimeGet(&time);
 329
 330         activity->TriggerYear   = time.Year;
 331         activity->TriggerMonth  = time.Month;
 332         activity->TriggerDay    = time.Day;
 333         activity->TriggerHour   = time.Hour;
 334         activity->TriggerMinute = time.Minute;
 335         activity->TriggerSecond = time.Second;
 336         activity->TriggerMillisecond = time.Millisecond;
 337     }
 338
 339     activity->ImageId  = ccd->ImageId;
 340     activity->SystemTS = xTaskGetTickCount() * portTICK_RATE_MS;
 341     {
 342         AttitudeStateData attitude;
 343         AttitudeStateGet(&attitude);
 344
 345         activity->Roll  = attitude.Roll;
 346         activity->Pitch = attitude.Pitch;
 347         activity->Yaw   = attitude.Yaw;
 348     }
 349 }
 350
 351 static void HomeLocationUpdateCb(__attribute__((unused)) UAVObjEvent *ev)
 352 {
 353     HomeLocationData home;
 354
 355     HomeLocationGet(&home);
 356
 357     int32_t LLAi[3] = {
 358         home.Latitude,
 359         home.Longitude,
 360         home.Altitude
 361     };
 362     LLA2ECEF(LLAi, ccd->HomeECEF);
 363     RneFromLLA(LLAi, ccd->HomeRne);
 364 }