LP-385 - Manage time/distance trigger, picture trigger pulse duration

flight/modules/CameraControl/cameracontrol.c

@@ -59,6 +59,7 @@ static struct CameraControl_data {
     CameraStatus outputStatus;
     CameraStatus lastOutputStatus;
     CameraStatus manualInput;
+    CameraStatus lastManualInput;
     bool autoTriggerEnabled;
 } *ccd;
 
@@ -94,6 +95,7 @@ int32_t CameraControlInitialize(void)
         CameraDesiredInitialize();
         CameraControlSettingsInitialize();
         CameraControlSettingsConnectCallback(SettingsUpdateCb);
+
         SettingsUpdateCb(NULL);
 
         // init output:
@@ -119,28 +121,55 @@ MODULE_INITCALL(CameraControlInitialize, CameraControlStart);
 
 static void CameraControlTask()
 {
+    bool trigger = false;
+    PositionStateData pos;
+
     if (checkActivation()) {
-        // Manual override
+        if (ccd->manualInput != ccd->lastManualInput && ccd->manualInput != CAMERASTATUS_Idle) {
-        if (ccd->manualInput != CAMERASTATUS_Idle) {
+            // Manual override
             ccd->outputStatus    = ccd->manualInput;
             ccd->activity.Reason = CAMERACONTROLACTIVITY_REASON_MANUAL;
         } else {
             if (ccd->autoTriggerEnabled) {
-                ccd->activity.Reason = CAMERACONTROLACTIVITY_REASON_AUTOTIME;
+                // check trigger conditions
-                ccd->outputStatus    = CAMERASTATUS_Shot;
+                if (ccd->settings.TimeInterval > 0) {
+                    if (PIOS_DELAY_DiffuS(ccd->lastTriggerTimeRaw) > ccd->settings.TimeInterval * (1000 * 1000)) {
+                        trigger = true;
+                        ccd->activity.Reason = CAMERACONTROLACTIVITY_REASON_AUTOTIME;
+                    }
+                }
+
+                if (ccd->settings.SpaceInterval > 0) {
+                    PositionStateGet(&pos);
+                    float dn = pos.North - ccd->lastTriggerNEDPosition[0];
+                    float de = pos.East - ccd->lastTriggerNEDPosition[1];
+                    float distance = sqrtf((dn * dn) + (de * de));
+                    ccd->activity.TriggerMilliSecond = (int16_t)distance * 10.0f;
+                    if (distance > ccd->settings.SpaceInterval) {
+                        trigger = true;
+                        ccd->activity.Reason = CAMERACONTROLACTIVITY_REASON_AUTODISTANCE;
+                    }
+                }
             }
         }
+    }
+    if (trigger) {
+        ccd->outputStatus = CAMERASTATUS_Shot;
+        ccd->lastTriggerTimeRaw = PIOS_DELAY_GetRaw();
+        ccd->lastTriggerNEDPosition[0] = pos.North;
+        ccd->lastTriggerNEDPosition[1] = pos.East;
+        ccd->lastTriggerNEDPosition[2] = pos.Down;
     } else {
         ccd->outputStatus = CAMERASTATUS_Idle;
+        ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_IDLE;
     }
 
-    UpdateOutput();
+    ccd->lastManualInput = ccd->manualInput;
     PublishActivity();
-    ccd->lastOutputStatus = ccd->outputStatus;
+    UpdateOutput();
     PIOS_CALLBACKSCHEDULER_Schedule(ccd->callbackHandle, CALLBACK_STD_PERIOD, CALLBACK_UPDATEMODE_SOONER);
 }
 
-
 static void SettingsUpdateCb(__attribute__((unused)) UAVObjEvent *ev)
 {
     CameraControlSettingsGet(&ccd->settings);
@@ -203,7 +232,14 @@ static void UpdateOutput()
     if (ccd->outputStatus != ccd->lastOutputStatus) {
         switch (ccd->outputStatus) {
         case CAMERASTATUS_Idle:
-            CameraDesiredTriggerSet(&ccd->settings.OutputValues.Idle);
+            if (CAMERASTATUS_Shot == ccd->lastOutputStatus) {
+                if (PIOS_DELAY_DiffuS(ccd->lastTriggerTimeRaw) > ccd->settings.TriggerPulseWidth * 1000) {
+                    CameraDesiredTriggerSet(&ccd->settings.OutputValues.Idle);
+                } else {
+                    // skip updating lastOutputStatus until TriggerPulseWidth elapsed
+                    return;
+                }
+            }
             break;
         case CAMERASTATUS_Shot:
             CameraDesiredTriggerSet(&ccd->settings.OutputValues.Shot);
@@ -213,6 +249,7 @@ static void UpdateOutput()
             break;
         }
     }
+    ccd->lastOutputStatus = ccd->outputStatus;
 }
 
 static void PublishActivity()
@@ -223,7 +260,7 @@ static void PublishActivity()
             if (ccd->lastOutputStatus == CAMERASTATUS_Video) {
                 ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_STOPVIDEO;
             } else {
-                ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_NONE;
+                ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_IDLE;
             }
             break;
         case CAMERASTATUS_Shot:
@@ -233,11 +270,12 @@ static void PublishActivity()
             if (ccd->lastOutputStatus != CAMERASTATUS_Video) {
                 ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_STARTVIDEO;
             } else {
-                ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_NONE;
+                ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_IDLE;
             }
             break;
         }
-        if (ccd->activity.Activity != CAMERACONTROLACTIVITY_ACTIVITY_NONE) {
+        if (ccd->activity.Activity != CAMERACONTROLACTIVITY_ACTIVITY_IDLE
+            || ccd->lastOutputStatus != CAMERASTATUS_Shot) {
             FillActivityInfo();
             CameraControlActivitySet(&ccd->activity);
         }
@@ -251,9 +289,9 @@ static void FillActivityInfo()
         PositionStateData position;
         PositionStateGet(&position);
 
-        activity->Latitude  = position.North;
+        activity->Latitude  = position.North * 1e3f;
-        activity->Longitude = position.East;
+        activity->Longitude = position.East * 1e3f;
-        activity->Altitude  = -position.Down;
+        activity->Altitude  = -position.Down * 1e3f;
     }
     {
         GPSTimeData time;
@@ -267,7 +305,8 @@ static void FillActivityInfo()
         activity->TriggerSecond = time.Second;
         activity->TriggerMilliSecond = time.MilliSecond;
     }
-        activity->SysTS = PIOS_DELAY_GetuS();
+
+    activity->SysTS = PIOS_DELAY_GetuS();
     {
         AttitudeStateData attitude;
         AttitudeStateGet(&attitude);
@@ -276,5 +315,4 @@ static void FillActivityInfo()
         activity->Pitch = attitude.Pitch;
         activity->Yaw   = attitude.Yaw;
     }
-
 }

shared/uavobjectdefinition/cameracontrolactivity.xml

@@ -14,7 +14,7 @@
         <field name="TriggerMinute" units="" type="int8" elements="1"/>
         <field name="TriggerSecond" units="" type="int8" elements="1"/>
         <field name="TriggerMilliSecond" units="" type="int16" elements="1"/>
-        <field name="Activity" units="" type="enum" elements="1" options="None,TriggerPicture,StartVideo,StopVideo" default="None"/>
+        <field name="Activity" units="" type="enum" elements="1" options="Idle,TriggerPicture,StartVideo,StopVideo" default="None"/>
         <field name="Reason" units="" type="enum" elements="1" options="Manual,AutoDistance,AutoTime" default="Manual"/>
         <field name="SysTS" units="us" type="uint32" elements="1"/>
         <access gcs="readwrite" flight="readwrite"/>

shared/uavobjectdefinition/cameracontrolsettings.xml

@@ -6,7 +6,7 @@
         <field name="AutoTriggerMode" units="mode" type="enum" elements="1" options="Disabled,WhenArmed,Always,Input,Mission" defaultvalue="WhenArmed"/>
         <field name="OutputValues" units="%" type="float" elementnames="Idle,Shot,Video" defaultvalue="0,-1,1" />
         <field name="InputValues" units="%" type="float" elementnames="Idle,Shot,Video" defaultvalue="0,-1,1" />
-        <field name="TriggerResponseTime" units="ms" type="uint8" elements="1" defaultvalue="0"/>
+        <field name="TriggerPulseWidth" units="ms" type="uint8" elements="1" defaultvalue="50"/>
         <field name="TimeInterval" units="s" type="float" elements="1" defaultvalue="0"/>
         <field name="SpaceInterval" units="m" type="float" elements="1" defaultvalue="0"/>
         <access gcs="readwrite" flight="readwrite"/>