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     HwSettingsOptionalModulesData modules;
  94     HwSettingsOptionalModulesGet(&modules);
  95     if (modules.CameraControl == HWSETTINGS_OPTIONALMODULES_ENABLED) {
  96         ccd = (struct CameraControl_data *)pios_malloc(sizeof(struct CameraControl_data));
  97         memset(ccd, 0, sizeof(struct CameraControl_data));
  98         ccd->callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&CameraControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_CAMERACONTROL, STACK_SIZE_BYTES);
  99         CameraControlActivityInitialize();
 100         CameraDesiredInitialize();
 101         CameraControlSettingsConnectCallback(SettingsUpdateCb);
 102         HomeLocationConnectCallback(HomeLocationUpdateCb);
 103         GPSTimeInitialize();
 104         PositionStateInitialize();
 105         AttitudeStateInitialize();
 106         AccessoryDesiredInitialize();
 107         FlightStatusInitialize();
 108
 109
 110         SettingsUpdateCb(NULL);
 111         HomeLocationUpdateCb(NULL);
 112
 113         // init output:
 114         ccd->outputStatus = CAMERASTATUS_Idle;
 115         UpdateOutput();
 116     }
 117     return 0;
 118 }
 119
 120 /* stub: module has no module thread */
 121 int32_t CameraControlStart(void)
 122 {
 123     if (!ccd) {
 124         return 0;
 125     }
 126
 127     PIOS_CALLBACKSCHEDULER_Schedule(ccd->callbackHandle, CALLBACK_STD_PERIOD, CALLBACK_UPDATEMODE_LATER);
 128     ccd->lastTriggerTimeRaw = PIOS_DELAY_GetRaw();
 129     return 0;
 130 }
 131
 132 MODULE_INITCALL(CameraControlInitialize, CameraControlStart);
 133
 134 static void CameraControlTask()
 135 {
 136     bool trigger = false;
 137     PositionStateData pos;
 138     uint32_t timeSinceLastShot = PIOS_DELAY_DiffuS(ccd->lastTriggerTimeRaw);
 139     CameraStatus newStatus;
 140
 141     if (checkActivation()) {
 142         if (ccd->manualInput != ccd->lastManualInput && ccd->manualInput != CAMERASTATUS_Idle) {
 143             // Manual override
 144             trigger   = true;
 145             newStatus = ccd->manualInput;
 146             ccd->activity.Reason = CAMERACONTROLACTIVITY_REASON_MANUAL;
 147         } else {
 148             // MinimumTimeInterval sets a hard limit on time between two consecutive shots, i.e. the minimum time between shots the camera can achieve
 149             if (ccd->autoTriggerEnabled &&
 150                 timeSinceLastShot > (ccd->settings.MinimumTimeInterval * 1000 * 1000)) {
 151                 // check trigger conditions
 152                 if (ccd->settings.TimeInterval > 0) {
 153                     if (timeSinceLastShot > ccd->settings.TimeInterval * (1000 * 1000)) {
 154                         trigger   = true;
 155                         newStatus = CAMERASTATUS_Shot;
 156                         ccd->activity.Reason = CAMERACONTROLACTIVITY_REASON_AUTOTIME;
 157                     }
 158                 }
 159
 160                 if (ccd->settings.SpaceInterval > 0) {
 161                     PositionStateGet(&pos);
 162                     float dn = pos.North - ccd->lastTriggerNEDPosition[0];
 163                     float de = pos.East - ccd->lastTriggerNEDPosition[1];
 164                     float distance = sqrtf((dn * dn) + (de * de));
 165                     ccd->activity.TriggerMillisecond = (int16_t)distance * 10.0f;
 166                     if (distance > ccd->settings.SpaceInterval) {
 167                         trigger   = true;
 168                         newStatus = CAMERASTATUS_Shot;
 169                         ccd->activity.Reason = CAMERACONTROLACTIVITY_REASON_AUTODISTANCE;
 170                     }
 171                 }
 172             }
 173         }
 174     }
 175     if (trigger) {
 176         ccd->outputStatus = newStatus;
 177         ccd->ImageId++;
 178         ccd->lastTriggerTimeRaw = PIOS_DELAY_GetRaw();
 179         ccd->lastTriggerNEDPosition[0] = pos.North;
 180         ccd->lastTriggerNEDPosition[1] = pos.East;
 181         ccd->lastTriggerNEDPosition[2] = pos.Down;
 182     } else {
 183         ccd->outputStatus = CAMERASTATUS_Idle;
 184         ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_IDLE;
 185     }
 186
 187     ccd->lastManualInput = ccd->manualInput;
 188     PublishActivity();
 189     UpdateOutput();
 190     PIOS_CALLBACKSCHEDULER_Schedule(ccd->callbackHandle, CALLBACK_STD_PERIOD, CALLBACK_UPDATEMODE_SOONER);
 191 }
 192
 193 static void SettingsUpdateCb(__attribute__((unused)) UAVObjEvent *ev)
 194 {
 195     CameraControlSettingsGet(&ccd->settings);
 196 }
 197
 198 static bool checkActivation()
 199 {
 200     if (ccd->settings.ManualTriggerInput != CAMERACONTROLSETTINGS_MANUALTRIGGERINPUT_NONE) {
 201         uint8_t accessory = ccd->settings.ManualTriggerInput - CAMERACONTROLSETTINGS_MANUALTRIGGERINPUT_ACCESSORY0;
 202
 203         AccessoryDesiredData accessoryDesired;
 204         AccessoryDesiredInstGet(accessory, &accessoryDesired);
 205
 206         if (fabsf(accessoryDesired.AccessoryVal - ccd->settings.InputValues.Shot) < INPUT_DEADBAND) {
 207             ccd->manualInput = CAMERASTATUS_Shot;
 208         } else if (fabsf(accessoryDesired.AccessoryVal - ccd->settings.InputValues.Video) < INPUT_DEADBAND) {
 209             ccd->manualInput = CAMERASTATUS_Video;
 210         } else {
 211             ccd->manualInput = CAMERASTATUS_Idle;
 212         }
 213     }
 214
 215     switch (ccd->settings.AutoTriggerMode) {
 216     case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_DISABLED:
 217         ccd->autoTriggerEnabled = false;
 218         break;
 219     case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_WHENARMED:
 220     {
 221         FlightStatusArmedOptions armed;
 222
 223         FlightStatusArmedGet(&armed);
 224         ccd->autoTriggerEnabled = (armed == FLIGHTSTATUS_ARMED_ARMED);
 225     }
 226     break;
 227     case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_ALWAYS:
 228         ccd->autoTriggerEnabled = true;
 229         break;
 230     case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_INPUT:
 231     {
 232         uint8_t accessory = ccd->settings.AutoTriggerInput - CAMERACONTROLSETTINGS_AUTOTRIGGERINPUT_ACCESSORY0;
 233         AccessoryDesiredData accessoryDesired;
 234         AccessoryDesiredInstGet(accessory, &accessoryDesired);
 235
 236         ccd->autoTriggerEnabled = (accessoryDesired.AccessoryVal > INPUT_DEADBAND);
 237     }
 238     break;
 239     case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_MISSION:
 240     {
 241         FlightStatusFlightModeOptions flightmode;
 242         FlightStatusFlightModeGet(&flightmode);
 243         ccd->autoTriggerEnabled = (flightmode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER);
 244     }
 245     break;
 246     }
 247     return ccd->autoTriggerEnabled || (ccd->manualInput != CAMERASTATUS_Idle);
 248 }
 249
 250 static void UpdateOutput()
 251 {
 252     if (ccd->outputStatus != ccd->lastOutputStatus) {
 253         switch (ccd->outputStatus) {
 254         case CAMERASTATUS_Idle:
 255             if (CAMERASTATUS_Shot == ccd->lastOutputStatus) {
 256                 if (PIOS_DELAY_DiffuS(ccd->lastTriggerTimeRaw) > ccd->settings.TriggerPulseWidth * 1000) {
 257                     CameraDesiredTriggerSet(&ccd->settings.OutputValues.Idle);
 258                 } else {
 259                     // skip updating lastOutputStatus until TriggerPulseWidth elapsed
 260                     return;
 261                 }
 262             }
 263             break;
 264         case CAMERASTATUS_Shot:
 265             CameraDesiredTriggerSet(&ccd->settings.OutputValues.Shot);
 266             break;
 267         case CAMERASTATUS_Video:
 268             CameraDesiredTriggerSet(&ccd->settings.OutputValues.Video);
 269             break;
 270         }
 271     }
 272     ccd->lastOutputStatus = ccd->outputStatus;
 273 }
 274
 275 static void PublishActivity()
 276 {
 277     if (ccd->outputStatus != ccd->lastOutputStatus) {
 278         switch (ccd->outputStatus) {
 279         case CAMERASTATUS_Idle:
 280             if (ccd->lastOutputStatus == CAMERASTATUS_Video) {
 281                 ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_STOPVIDEO;
 282             } else {
 283                 ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_IDLE;
 284             }
 285             break;
 286         case CAMERASTATUS_Shot:
 287             ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_TRIGGERPICTURE;
 288             break;
 289         case CAMERASTATUS_Video:
 290             if (ccd->lastOutputStatus != CAMERASTATUS_Video) {
 291                 ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_STARTVIDEO;
 292             } else {
 293                 ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_IDLE;
 294             }
 295             break;
 296         }
 297         if (ccd->activity.Activity != CAMERACONTROLACTIVITY_ACTIVITY_IDLE
 298             || ccd->lastOutputStatus != CAMERASTATUS_Shot) {
 299             FillActivityInfo();
 300             CameraControlActivitySet(&ccd->activity);
 301         }
 302     }
 303 }
 304
 305 static void FillActivityInfo()
 306 {
 307     CameraControlActivityData *activity = &ccd->activity;
 308     {
 309         PositionStateData position;
 310         PositionStateGet(&position);
 311         int32_t LLAi[3];
 312         const float pos[3] = {
 313             position.North,
 314             position.East,
 315             position.Down
 316         };
 317         Base2LLA(pos, ccd->HomeECEF, ccd->HomeRne, LLAi);
 318
 319         activity->Latitude  = LLAi[0];
 320         activity->Longitude = LLAi[1];
 321         activity->Altitude  = ((float)LLAi[2]) * 1e-4f;
 322     }
 323     {
 324         GPSTimeData time;
 325         GPSTimeGet(&time);
 326
 327         activity->TriggerYear   = time.Year;
 328         activity->TriggerMonth  = time.Month;
 329         activity->TriggerDay    = time.Day;
 330         activity->TriggerHour   = time.Hour;
 331         activity->TriggerMinute = time.Minute;
 332         activity->TriggerSecond = time.Second;
 333         activity->TriggerMillisecond = time.Millisecond;
 334     }
 335
 336     activity->ImageId  = ccd->ImageId;
 337     activity->SystemTS = xTaskGetTickCount() * portTICK_RATE_MS;
 338     {
 339         AttitudeStateData attitude;
 340         AttitudeStateGet(&attitude);
 341
 342         activity->Roll  = attitude.Roll;
 343         activity->Pitch = attitude.Pitch;
 344         activity->Yaw   = attitude.Yaw;
 345     }
 346 }
 347
 348 static void HomeLocationUpdateCb(__attribute__((unused)) UAVObjEvent *ev)
 349 {
 350     HomeLocationData home;
 351
 352     HomeLocationGet(&home);
 353
 354     int32_t LLAi[3] = {
 355         home.Latitude,
 356         home.Longitude,
 357         home.Altitude
 358     };
 359     LLA2ECEF(LLAi, ccd->HomeECEF);
 360     RneFromLLA(LLAi, ccd->HomeRne);
 361 }