rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

LP_385 - Implement activation/manual override logic, Camera activity "publishing"

Browse Source
This commit is contained in:
Alessio Morale 2016-08-18 19:25:55 +02:00
parent c7e060c04a
commit 88a8e767c4
5 changed files with 232 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

227
flight/modules/CameraControl/cameracontrol.c
View File

@ -33,16 +33,47 @@
#include <cameradesired.h>
#include <cameracontrolsettings.h>
#include <cameracontrolactivity.h>
#include <accessorydesired.h>
#include <attitudestate.h>
#include <callbackinfo.h>
#include <flightstatus.h>
#include <gpstime.h>
#include <hwsettings.h>
#include <positionstate.h>
#include <velocitystate.h>
/*
// Private variables
static struct CameraControl_data {
portTickType lastSysTime;
} *ccd;
*/
//static void setOutput();
typedef enum {
CAMERASTATUS_Idle = 0,
CAMERASTATUS_Shot,
CAMERASTATUS_Video
} CameraStatus;
static struct CameraControl_data {
int32_t lastTriggerTimeRaw;
float lastTriggerNEDPosition[3];
CameraControlSettingsData settings;
CameraControlActivityData activity;
DelayedCallbackInfo *callbackHandle;
CameraStatus outputStatus;
CameraStatus lastOutputStatus;
CameraStatus manualInput;
bool autoTriggerEnabled;
} *ccd;
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define CBTASK_PRIORITY CALLBACK_TASK_AUXILIARY
#define STACK_SIZE_BYTES 512
#define CALLBACK_STD_PERIOD 50
#define INPUT_DEADBAND 0.1f
static void CameraControlTask();
static void SettingsUpdateCb(__attribute__((unused)) UAVObjEvent *ev);
static void UpdateOutput();
static void PublishActivity();
static bool checkActivation();
static void FillActivityInfo();
/**
@ -51,13 +82,197 @@ static struct CameraControl_data {
*/
int32_t CameraControlInitialize(void)
{
ccd = 0;
HwSettingsInitialize();
HwSettingsOptionalModulesData modules;
HwSettingsOptionalModulesGet(&modules);
if (modules.CameraControl == HWSETTINGS_OPTIONALMODULES_ENABLED) {
ccd = (struct CameraControl_data *)pios_malloc(sizeof(struct CameraControl_data));
memset(ccd, 0, sizeof(struct CameraControl_data));
ccd->callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&CameraControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_CAMERACONTROL, STACK_SIZE_BYTES);
CameraControlActivityInitialize();
CameraDesiredInitialize();
CameraControlSettingsInitialize();
CameraControlSettingsConnectCallback(SettingsUpdateCb);
SettingsUpdateCb(NULL);
// init output:
ccd->outputStatus = CAMERASTATUS_Idle;
UpdateOutput();
}
return 0;
}
/* stub: module has no module thread */
int32_t CameraControlStart(void)
{
if (!ccd) {
return 0;
}
PIOS_CALLBACKSCHEDULER_Schedule(ccd->callbackHandle, CALLBACK_STD_PERIOD, CALLBACK_UPDATEMODE_LATER);
ccd->lastTriggerTimeRaw = PIOS_DELAY_GetRaw();
return 0;
}
MODULE_INITCALL(CameraControlInitialize, CameraControlStart);
static void CameraControlTask()
{
if (checkActivation()) {
// Manual override
if (ccd->manualInput != CAMERASTATUS_Idle) {
ccd->outputStatus = ccd->manualInput;
ccd->activity.Reason = CAMERACONTROLACTIVITY_REASON_MANUAL;
} else {
if (ccd->autoTriggerEnabled) {
ccd->activity.Reason = CAMERACONTROLACTIVITY_REASON_AUTOTIME;
ccd->outputStatus = CAMERASTATUS_Shot;
}
}
} else {
ccd->outputStatus = CAMERASTATUS_Idle;
}
UpdateOutput();
PublishActivity();
ccd->lastOutputStatus = ccd->outputStatus;
PIOS_CALLBACKSCHEDULER_Schedule(ccd->callbackHandle, CALLBACK_STD_PERIOD, CALLBACK_UPDATEMODE_SOONER);
}
static void SettingsUpdateCb(__attribute__((unused)) UAVObjEvent *ev)
{
CameraControlSettingsGet(&ccd->settings);
}
static bool checkActivation()
{
if (ccd->settings.ManualTriggerInput != CAMERACONTROLSETTINGS_MANUALTRIGGERINPUT_NONE) {
uint8_t accessory = ccd->settings.ManualTriggerInput - CAMERACONTROLSETTINGS_MANUALTRIGGERINPUT_ACCESSORY0;
AccessoryDesiredData accessoryDesired;
AccessoryDesiredInstGet(accessory, &accessoryDesired);
if (fabsf(accessoryDesired.AccessoryVal - ccd->settings.InputValues.Shot) < INPUT_DEADBAND) {
ccd->manualInput = CAMERASTATUS_Shot;
} else if (fabsf(accessoryDesired.AccessoryVal - ccd->settings.InputValues.Video) < INPUT_DEADBAND) {
ccd->manualInput = CAMERASTATUS_Video;
} else {
ccd->manualInput = CAMERASTATUS_Idle;
}
}
switch (ccd->settings.AutoTriggerMode) {
case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_DISABLED:
ccd->autoTriggerEnabled = false;
break;
case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_WHENARMED:
{
FlightStatusArmedOptions armed;
FlightStatusArmedGet(&armed);
ccd->autoTriggerEnabled = (armed == FLIGHTSTATUS_ARMED_ARMED);
}
break;
case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_ALWAYS:
ccd->autoTriggerEnabled = true;
break;
case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_INPUT:
{
uint8_t accessory = ccd->settings.AutoTriggerInput - CAMERACONTROLSETTINGS_AUTOTRIGGERINPUT_ACCESSORY0;
AccessoryDesiredData accessoryDesired;
AccessoryDesiredInstGet(accessory, &accessoryDesired);
ccd->autoTriggerEnabled = (accessoryDesired.AccessoryVal > INPUT_DEADBAND);
}
break;
case CAMERACONTROLSETTINGS_AUTOTRIGGERMODE_MISSION:
{
FlightStatusFlightModeOptions flightmode;
FlightStatusFlightModeGet(&flightmode);
ccd->autoTriggerEnabled = (flightmode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER);
}
break;
}
return ccd->autoTriggerEnabled || (ccd->manualInput != CAMERASTATUS_Idle);
}
static void UpdateOutput()
{
if (ccd->outputStatus != ccd->lastOutputStatus) {
switch (ccd->outputStatus) {
case CAMERASTATUS_Idle:
CameraDesiredTriggerSet(&ccd->settings.OutputValues.Idle);
break;
case CAMERASTATUS_Shot:
CameraDesiredTriggerSet(&ccd->settings.OutputValues.Shot);
break;
case CAMERASTATUS_Video:
CameraDesiredTriggerSet(&ccd->settings.OutputValues.Video);
break;
}
}
}
static void PublishActivity()
{
if (ccd->outputStatus != ccd->lastOutputStatus) {
switch (ccd->outputStatus) {
case CAMERASTATUS_Idle:
if (ccd->lastOutputStatus == CAMERASTATUS_Video) {
ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_STOPVIDEO;
} else {
ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_NONE;
}
break;
case CAMERASTATUS_Shot:
ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_TRIGGERPICTURE;
break;
case CAMERASTATUS_Video:
if (ccd->lastOutputStatus != CAMERASTATUS_Video) {
ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_STARTVIDEO;
} else {
ccd->activity.Activity = CAMERACONTROLACTIVITY_ACTIVITY_NONE;
}
break;
}
if (ccd->activity.Activity != CAMERACONTROLACTIVITY_ACTIVITY_NONE) {
FillActivityInfo();
CameraControlActivitySet(&ccd->activity);
}
}
}
static void FillActivityInfo()
{
CameraControlActivityData *activity = &ccd->activity;
{
PositionStateData position;
PositionStateGet(&position);
activity->Latitude = position.North;
activity->Longitude = position.East;
activity->Altitude = -position.Down;
}
{
GPSTimeData time;
GPSTimeGet(&time);
activity->TriggerYear = time.Year;
activity->TriggerMonth = time.Month;
activity->TriggerDay = time.Day;
activity->TriggerHour = time.Hour;
activity->TriggerMinute = time.Minute;
activity->TriggerSecond = time.Second;
}
activity->SysTS = PIOS_DELAY_GetuS();
{
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
activity->Roll = attitude.Roll;
activity->Pitch = attitude.Pitch;
activity->Yaw = attitude.Yaw;
}
}

1
flight/targets/boards/revolution/firmware/Makefile
View File

@ -42,6 +42,7 @@ MODULES += Actuator
MODULES += GPS
MODULES += TxPID
MODULES += CameraStab
MODULES += CameraControl
MODULES += Battery
MODULES += FirmwareIAP
MODULES += Radio

3
shared/uavobjectdefinition/callbackinfo.xml
View File

@ -12,6 +12,7 @@
<elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname>
<elementname>CameraControl</elementname>
</elementnames>
</field>
<field name="Running" units="bool" type="enum">
@ -25,6 +26,7 @@
<elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname>
<elementname>CameraControl</elementname>
</elementnames>
<options>
<option>False</option>
@ -42,6 +44,7 @@
<elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname>
<elementname>CameraControl</elementname>
</elementnames>
</field>
<access gcs="readonly" flight="readwrite"/>

3
shared/uavobjectdefinition/cameracontrolactivity.xml
View File

@ -14,8 +14,9 @@
<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,TriggerVideo" default="None"/>
<field name="Activity" units="" type="enum" elements="1" options="None,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"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>

8
shared/uavobjectdefinition/cameracontrolsettings.xml
View File

@ -1,9 +1,11 @@
<xml>
<object name="CameraControlSettings" singleinstance="true" settings="true" category="Control">
<description>Settings for the @ref CameraControl module</description>
<field name="Input" units="channel" type="enum" elements="1" options="Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5,None" defaultvalue="None"/>
<field name="AutoTriggerMode" units="mode" type="enum" elements="1" options="Disabled,WhenArmed,Always,Input" defaultvalue="Armed"/>
<field name="OutputValues" units="%" type="float" elementnames="Standby,Shot,Video" defaultvalue="0.5,1.0,0" />
<field name="AutoTriggerInput" units="channel" type="enum" elements="1" options="None,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="None"/>
<field name="ManualTriggerInput" units="channel" type="enum" elements="1" options="None,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="None"/>
<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="TimeInterval" units="s" type="float" elements="1" defaultvalue="0"/>
<field name="SpaceInterval" units="m" type="float" elements="1" defaultvalue="0"/>