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

Merged in alessiomorale/librepilot/LP-385_cameracontrol (pull request #309)

Browse Source 
Lp 385_cameracontrol
This commit is contained in:
Lalanne Laurent 2016-09-21 21:24:05 +02:00
commit 553448abad
26 changed files with 655 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

139
flight/libraries/CoordinateConversions.c
View File

@ -27,25 +27,29 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <math.h>
#include <stdint.h>
#include <pios_math.h>
#include "CoordinateConversions.h"
#include "inc/CoordinateConversions.h"
#define MIN_ALLOWABLE_MAGNITUDE 1e-30f
// Equatorial Radius
#define equatorial_radius 6378137.0d
// Eccentricity
#define eccentricity 8.1819190842622e-2d
#define equatorial_radius_sq (equatorial_radius * equatorial_radius)
#define eccentricity_sq (eccentricity * eccentricity)
// ****** convert Lat,Lon,Alt to ECEF ************
void LLA2ECEF(int32_t LLAi[3], double ECEF[3])
void LLA2ECEF(const int32_t LLAi[3], float ECEF[3])
{
const double a = 6378137.0d; // Equatorial Radius
const double e = 8.1819190842622e-2d; // Eccentricity
const double e2 = e * e; // Eccentricity squared
double sinLat, sinLon, cosLat, cosLon;
double N;
double LLA[3] = {
(double)LLAi[0] * 1e-7d,
(double)LLAi[1] * 1e-7d,
(double)LLAi[2] * 1e-4d
double LLA[3] = {
((double)LLAi[0]) * 1e-7d,
((double)LLAi[1]) * 1e-7d,
((double)LLAi[2]) * 1e-4d
};
sinLat = sin(DEG2RAD_D(LLA[0]));
@ -53,60 +57,53 @@ void LLA2ECEF(int32_t LLAi[3], double ECEF[3])
cosLat = cos(DEG2RAD_D(LLA[0]));
cosLon = cos(DEG2RAD_D(LLA[1]));
N = a / sqrt(1.0d - e2 * sinLat * sinLat); // prime vertical radius of curvature
N = equatorial_radius / sqrt(1.0d - eccentricity_sq * sinLat * sinLat); // prime vertical radius of curvature
ECEF[0] = (N + LLA[2]) * cosLat * cosLon;
ECEF[1] = (N + LLA[2]) * cosLat * sinLon;
ECEF[2] = ((1.0d - e2) * N + LLA[2]) * sinLat;
ECEF[0] = (float)((N + LLA[2]) * cosLat * cosLon);
ECEF[1] = (float)((N + LLA[2]) * cosLat * sinLon);
ECEF[2] = (float)(((1.0d - eccentricity_sq) * N + LLA[2]) * sinLat);
}
// ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
uint16_t ECEF2LLA(double ECEF[3], float LLA[3])
void ECEF2LLA(const float ECEF[3], int32_t LLA[3])
{
/**
* LLA parameter is used to prime the iteration.
* A position within 1 meter of the specified LLA
* will be calculated within at most 3 iterations.
* If unknown: Call with any valid LLA coordinate
* will compute within at most 5 iterations.
* Suggestion: [0,0,0]
**/
/* b = math.sqrt( asq * (1-esq) )
bsq = b*b
const double a = 6378137.0f; // Equatorial Radius
const double e = 8.1819190842622e-2f; // Eccentricity
double x = ECEF[0], y = ECEF[1], z = ECEF[2];
double Lat, N, NplusH, delta, esLat;
uint16_t iter;
ep = math.sqrt((asq - bsq)/bsq)
p = math.sqrt( math.pow(x,2) + math.pow(y,2) )
th = math.atan2(a*z, b*p)
#define MAX_ITER 10 // should not take more than 5 for valid coordinates
#define ACCURACY 1.0e-11d // used to be e-14, but we don't need sub micrometer exact calculations
lon = math.atan2(y,x)
lat = math.atan2( (z + ep*ep *b * math.pow(math.sin(th),3) ), (p - esq*a*math.pow(math.cos(th),3)) )
N = a/( math.sqrt(1-esq*math.pow(math.sin(lat),2)) )
alt = p / math.cos(lat) - N*/
LLA[1] = (float)RAD2DEG_D(atan2(y, x));
Lat = DEG2RAD_D((double)LLA[0]);
esLat = e * sin(Lat);
N = a / sqrt(1 - esLat * esLat);
NplusH = N + (double)LLA[2];
delta = 1;
iter = 0;
const double x = ECEF[0], y = ECEF[1], z = ECEF[2];
while (((delta > ACCURACY) || (delta < -ACCURACY))
&& (iter < MAX_ITER)) {
delta = Lat - atan(z / (sqrt(x * x + y * y) * (1 - (N * e * e / NplusH))));
Lat = Lat - delta;
esLat = e * sin(Lat);
N = a / sqrt(1 - esLat * esLat);
NplusH = sqrt(x * x + y * y) / cos(Lat);
iter += 1;
}
const double b = sqrt(equatorial_radius_sq * (1 - eccentricity_sq));
const double bsq = b * b;
const double ep = sqrt((equatorial_radius_sq - bsq) / bsq);
LLA[0] = RAD2DEG_D(Lat);
LLA[2] = NplusH - N;
const double p = sqrt(x * x + y * y);
const double th = atan2(equatorial_radius * z, b * p);
return iter < MAX_ITER;
double lon = atan2(y, x);
const double lat = atan2(
(z + ep * ep * b * pow(sin(th), 3)),
(p - eccentricity_sq * equatorial_radius * pow(cos(th), 3))
);
const double N = equatorial_radius / (sqrt(1 - eccentricity_sq * pow(sin(lat), 2)));
const double alt = p / cos(lat) - N;
LLA[0] = (int32_t)(RAD2DEG_D(lat) * 1e7d);
LLA[1] = (int32_t)(RAD2DEG_D(lon) * 1e7d) % ((int32_t)(180 * 1e7d));
LLA[2] = (int32_t)(alt * 1e4d);
}
// ****** find ECEF to NED rotation matrix ********
void RneFromLLA(int32_t LLAi[3], float Rne[3][3])
void RneFromLLA(const int32_t LLAi[3], float Rne[3][3])
{
float sinLat, sinLon, cosLat, cosLon;
@ -249,16 +246,30 @@ void Quaternion2zB(const float q[4], float z[3])
// ****** Express LLA in a local NED Base Frame ********
void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3])
void LLA2Base(const int32_t LLAi[3], const float BaseECEF[3], float Rne[3][3], float NED[3])
{
double ECEF[3];
float diff[3];
float ECEF[3];
LLA2ECEF(LLAi, ECEF);
ECEF2Base(ECEF, BaseECEF, Rne, NED);
}
diff[0] = (float)(ECEF[0] - BaseECEF[0]);
diff[1] = (float)(ECEF[1] - BaseECEF[1]);
diff[2] = (float)(ECEF[2] - BaseECEF[2]);
// ****** Express LLA in a local NED Base Frame ********
void Base2LLA(const float NED[3], const float BaseECEF[3], float Rne[3][3], int32_t LLAi[3])
{
float ECEF[3];
Base2ECEF(NED, BaseECEF, Rne, ECEF);
ECEF2LLA(ECEF, LLAi);
}
// ****** Express ECEF in a local NED Base Frame ********
void ECEF2Base(const float ECEF[3], const float BaseECEF[3], float Rne[3][3], float NED[3])
{
float diff[3];
diff[0] = (ECEF[0] - BaseECEF[0]);
diff[1] = (ECEF[1] - BaseECEF[1]);
diff[2] = (ECEF[2] - BaseECEF[2]);
NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2];
NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2];
@ -266,19 +277,21 @@ void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3]
}
// ****** Express ECEF in a local NED Base Frame ********
void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3])
void Base2ECEF(const float NED[3], const float BaseECEF[3], float Rne[3][3], float ECEF[3])
{
float diff[3];
diff[0] = (float)(ECEF[0] - BaseECEF[0]);
diff[1] = (float)(ECEF[1] - BaseECEF[1]);
diff[2] = (float)(ECEF[2] - BaseECEF[2]);
diff[0] = Rne[0][0] * NED[0] + Rne[1][0] * NED[1] + Rne[2][0] * NED[2];
diff[1] = Rne[0][1] * NED[0] + Rne[1][1] * NED[1] + Rne[2][1] * NED[2];
diff[2] = Rne[0][2] * NED[0] + Rne[1][2] * NED[1] + Rne[2][2] * NED[2];
NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2];
NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2];
NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2];
ECEF[0] = diff[0] + BaseECEF[0];
ECEF[1] = diff[1] + BaseECEF[1];
ECEF[2] = diff[2] + BaseECEF[2];
}
// ****** convert Rotation Matrix to Quaternion ********
// ****** if R converts from e to b, q is rotation from e to b ****
void R2Quaternion(float R[3][3], float q[4])

23
flight/libraries/inc/CoordinateConversions.h
View File

@ -29,14 +29,23 @@
#ifndef COORDINATECONVERSIONS_H_
#define COORDINATECONVERSIONS_H_
#include <math.h>
// ****** convert Lat,Lon,Alt to ECEF ************
void LLA2ECEF(int32_t LLAi[3], double ECEF[3]);
void LLA2ECEF(const int32_t LLAi[3], float ECEF[3]);
// ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
uint16_t ECEF2LLA(double ECEF[3], float LLA[3]);
// ****** convert ECEF to Lat,Lon,Alt *********
void ECEF2LLA(const float ECEF[3], int32_t LLA[3]);
void RneFromLLA(int32_t LLAi[3], float Rne[3][3]);
void RneFromLLA(const int32_t LLAi[3], float Rne[3][3]);
// ****** Express LLA in a local NED Base Frame and back ********
void LLA2Base(const int32_t LLAi[3], const float BaseECEF[3], float Rne[3][3], float NED[3]);
void Base2LLA(const float NED[3], const float BaseECEF[3], float Rne[3][3], int32_t LLAi[3]);
// ****** Express ECEF in a local NED Base Frame and back ********
void ECEF2Base(const float ECEF[3], const float BaseECEF[3], float Rne[3][3], float NED[3]);
void Base2ECEF(const float NED[3], const float BaseECEF[3], float Rne[3][3], float ECEF[3]);
// ****** find rotation matrix from rotation vector
void Rv2Rot(float Rv[3], float R[3][3]);
@ -65,12 +74,6 @@ void Quaternion2yB(const float q[4], float y[3]);
void QuaternionC2zB(const float q0, const float q1, const float q2, const float q3, float z[3]);
void Quaternion2zB(const float q[4], float z[3]);
// ****** Express LLA in a local NED Base Frame ********
void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3]);
// ****** Express ECEF in a local NED Base Frame ********
void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3]);
// ****** convert Rotation Matrix to Quaternion ********
// ****** if R converts from e to b, q is rotation from e to b ****
void R2Quaternion(float R[3][3], float q[4]);

13
flight/modules/Actuator/actuator.c
View File

@ -89,6 +89,7 @@ static xTaskHandle taskHandle;
static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
static SystemSettingsThrustControlOptions thrustType = SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE;
static bool camStabEnabled;
static bool camControlEnabled;
static uint8_t pinsMode[MAX_MIX_ACTUATORS];
// used to inform the actuator thread that actuator update rate is changed
@ -165,8 +166,8 @@ int32_t ActuatorInitialize()
HwSettingsOptionalModulesData optionalModules;
HwSettingsInitialize();
HwSettingsOptionalModulesGet(&optionalModules);
camStabEnabled = (optionalModules.CameraStab == HWSETTINGS_OPTIONALMODULES_ENABLED);
camStabEnabled = (optionalModules.CameraStab == HWSETTINGS_OPTIONALMODULES_ENABLED);
camControlEnabled = (optionalModules.CameraControl == HWSETTINGS_OPTIONALMODULES_ENABLED);
// Primary output of this module
ActuatorCommandInitialize();
@ -483,6 +484,14 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
command.Channel[ct] = 0;
}
}
if (mixer_type == MIXERSETTINGS_MIXER1TYPE_CAMERATRIGGER) {
if (camControlEnabled) {
CameraDesiredTriggerGet(&status[ct]);
} else {
status[ct] = 0;
}
}
}
// If mixer type is motor we need to find which motor has the highest value and which motor has the lowest value.

364
flight/modules/CameraControl/cameracontrol.c Normal file
View File

@ -0,0 +1,364 @@
/**
******************************************************************************
*
* @file cameracontrol.c
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* @brief camera control module. triggers cameras with multiple options
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
* Additional note on redistribution: The copyright and license notices above
* must be maintained in each individual source file that is a derivative work
* of this source file; otherwise redistribution is prohibited.
*/
#include <openpilot.h>
#include "inc/cameracontrol.h"
#include <CoordinateConversions.h>
#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 <homelocation.h>
#include <hwsettings.h>
#include <positionstate.h>
#include <velocitystate.h>
// Private variables
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;
CameraStatus lastManualInput;
bool autoTriggerEnabled;
uint16_t ImageId;
float HomeECEF[3];
float HomeRne[3][3];
} *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 HomeLocationUpdateCb(__attribute__((unused)) UAVObjEvent *ev);
static void UpdateOutput();
static void PublishActivity();
static bool checkActivation();
static void FillActivityInfo();
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
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);
HomeLocationInitialize();
HomeLocationConnectCallback(HomeLocationUpdateCb);
GPSTimeInitialize();
PositionStateInitialize();
AttitudeStateInitialize();
AccessoryDesiredInitialize();
FlightStatusInitialize();
SettingsUpdateCb(NULL);
HomeLocationUpdateCb(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()
{
bool trigger = false;
PositionStateData pos;
uint32_t timeSinceLastShot = PIOS_DELAY_DiffuS(ccd->lastTriggerTimeRaw);
CameraStatus newStatus;
if (checkActivation()) {
if (ccd->manualInput != ccd->lastManualInput && ccd->manualInput != CAMERASTATUS_Idle) {
// Manual override
trigger = true;
newStatus = ccd->manualInput;
ccd->activity.Reason = CAMERACONTROLACTIVITY_REASON_MANUAL;
} else {
// MinimumTimeInterval sets a hard limit on time between two consecutive shots, i.e. the minimum time between shots the camera can achieve
if (ccd->autoTriggerEnabled &&
timeSinceLastShot > (ccd->settings.MinimumTimeInterval * 1000 * 1000)) {
// check trigger conditions
if (ccd->settings.TimeInterval > 0) {
if (timeSinceLastShot > ccd->settings.TimeInterval * (1000 * 1000)) {
trigger = true;
newStatus = CAMERASTATUS_Shot;
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;
newStatus = CAMERASTATUS_Shot;
ccd->activity.Reason = CAMERACONTROLACTIVITY_REASON_AUTODISTANCE;
}
}
}
}
}
if (trigger) {
ccd->outputStatus = newStatus;
ccd->ImageId++;
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;
}
ccd->lastManualInput = ccd->manualInput;
PublishActivity();
UpdateOutput();
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:
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);
break;
case CAMERASTATUS_Video:
CameraDesiredTriggerSet(&ccd->settings.OutputValues.Video);
break;
}
}
ccd->lastOutputStatus = ccd->outputStatus;
}
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_IDLE;
}
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_IDLE;
}
break;
}
if (ccd->activity.Activity != CAMERACONTROLACTIVITY_ACTIVITY_IDLE
|| ccd->lastOutputStatus != CAMERASTATUS_Shot) {
FillActivityInfo();
CameraControlActivitySet(&ccd->activity);
}
}
}
static void FillActivityInfo()
{
CameraControlActivityData *activity = &ccd->activity;
{
PositionStateData position;
PositionStateGet(&position);
int32_t LLAi[3];
const float pos[3] = {
position.North,
position.East,
position.Down
};
Base2LLA(pos, ccd->HomeECEF, ccd->HomeRne, LLAi);
activity->Latitude = LLAi[0];
activity->Longitude = LLAi[1];
activity->Altitude = ((float)LLAi[2]) * 1e-4f;
}
{
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->TriggerMillisecond = time.Millisecond;
}
activity->ImageId = ccd->ImageId;
activity->SystemTS = xTaskGetTickCount() * portTICK_RATE_MS;
{
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
activity->Roll = attitude.Roll;
activity->Pitch = attitude.Pitch;
activity->Yaw = attitude.Yaw;
}
}
static void HomeLocationUpdateCb(__attribute__((unused)) UAVObjEvent *ev)
{
HomeLocationData home;
HomeLocationGet(&home);
int32_t LLAi[3] = {
home.Latitude,
home.Longitude,
home.Altitude
};
LLA2ECEF(LLAi, ccd->HomeECEF);
RneFromLLA(LLAi, ccd->HomeRne);
}

36
flight/modules/CameraControl/inc/cameracontrol.h Normal file
View File

@ -0,0 +1,36 @@
/**
******************************************************************************
*
* @file cameracontrol.h
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* @brief camera control module. triggers cameras with multiple options
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
* Additional note on redistribution: The copyright and license notices above
* must be maintained in each individual source file that is a derivative work
* of this source file; otherwise redistribution is prohibited.
*/
#ifndef FLIGHT_MODULES_CAMERACONTROL_INC_CAMERACONTROL_H
#define FLIGHT_MODULES_CAMERACONTROL_INC_CAMERACONTROL_H
#endif /* FLIGHT_MODULES_CAMERACONTROL_INC_CAMERACONTROL_H */

2
flight/modules/GPS/UBX.c
View File

@ -439,7 +439,7 @@ static void parse_ubx_nav_timeutc(struct UBXPacket *ubx, __attribute__((unused))
GpsTime.Hour = timeutc->hour;
GpsTime.Minute = timeutc->min;
GpsTime.Second = timeutc->sec;
GpsTime.Millisecond = (int16_t)(timeutc->nano / 1000000);
GPSTimeSet(&GpsTime);
} else {
// Time is not valid, nothing to do

2
flight/modules/StateEstimation/filterlla.c
View File

@ -44,7 +44,7 @@
struct data {
GPSSettingsData settings;
HomeLocationData home;
double HomeECEF[3];
float HomeECEF[3];
float HomeRne[3][3];
};

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

2
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -112,6 +112,8 @@ UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += receiverstatus
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += cameracontrolsettings
UAVOBJSRCFILENAMES += cameracontrolactivity
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += oplinksettings
UAVOBJSRCFILENAMES += oplinkstatus

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

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

2
flight/targets/boards/revonano/firmware/UAVObjects.inc
View File

@ -112,6 +112,8 @@ UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += receiverstatus
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += cameracontrolsettings
UAVOBJSRCFILENAMES += cameracontrolactivity
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += oplinksettings
UAVOBJSRCFILENAMES += oplinkstatus

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

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

6
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -21,9 +21,9 @@
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += statusgrounddrive
UAVOBJSRCFILENAMES += statusvtolland
UAVOBJSRCFILENAMES += statusvtolautotakeoff
UAVOBJSRCFILENAMES += pidstatus
UAVOBJSRCFILENAMES += statusvtolland
UAVOBJSRCFILENAMES += vtolselftuningstats
UAVOBJSRCFILENAMES += accelgyrosettings
UAVOBJSRCFILENAMES += accessorydesired
@ -112,6 +112,8 @@ UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += receiverstatus
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += cameracontrolsettings
UAVOBJSRCFILENAMES += cameracontrolactivity
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += oplinksettings
UAVOBJSRCFILENAMES += oplinkstatus
@ -125,7 +127,7 @@ UAVOBJSRCFILENAMES += mpugyroaccelsettings
UAVOBJSRCFILENAMES += txpidsettings
UAVOBJSRCFILENAMES += txpidstatus
UAVOBJSRCFILENAMES += takeofflocation
# UAVOBJSRCFILENAMES += perfcounter
UAVOBJSRCFILENAMES += perfcounter
UAVOBJSRCFILENAMES += systemidentsettings
UAVOBJSRCFILENAMES += systemidentstate

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

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

2
flight/targets/boards/sparky2/firmware/UAVObjects.inc
View File

@ -112,6 +112,8 @@ UAVOBJSRCFILENAMES += receiveractivity
UAVOBJSRCFILENAMES += receiverstatus
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRCFILENAMES += cameracontrolsettings
UAVOBJSRCFILENAMES += cameracontrolactivity
UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += oplinksettings
UAVOBJSRCFILENAMES += oplinkstatus

5
flight/tests/math/Makefile
View File

@ -32,7 +32,10 @@ endif
include $(FLIGHT_ROOT_DIR)/make/firmware-defs.mk
EXTRAINCDIRS += $(TOPDIR)
EXTRAINCDIRS += $(FLIGHT_ROOT_DIR)/flight/libraries/math
EXTRAINCDIRS += $(FLIGHTLIB)/math
EXTRAINCDIRS += $(FLIGHTLIB)
EXTRAINCDIRS += $(PIOS)/inc
SRC += $(FLIGHTLIB)/CoordinateConversions.c
include $(FLIGHT_ROOT_DIR)/make/unittest.mk

82
flight/tests/math/coordinateconversiontest.cpp Normal file
View File

@ -0,0 +1,82 @@
#include "gtest/gtest.h"
#define __STDC_WANT_DEC_FP__ /* Tell implementation that we want Decimal FP */
#include <stdio.h> /* printf */
#include <stdlib.h> /* abort */
#include <string.h> /* memset */
extern "C" {
#include <inc/CoordinateConversions.h>
}
#define epsilon_deg 0.00001f
#define epsilon_int_deg ((int32_t)(epsilon_deg * 1e7))
#define epsilon_metric 0.2f
#define epsilon_int_metric ((int32_t)(epsilon_metric * 1e4))
// To use a test fixture, derive a class from testing::Test.
class CoordinateConversionsTestRaw : public testing::Test {};
// ****** convert Lat,Lon,Alt to ECEF ************
// void LLA2ECEF(const int32_t LLAi[3], float ECEF[3]);
// ****** convert ECEF to Lat,Lon,Alt *********
// void ECEF2LLA(const float ECEF[3], int32_t LLA[3]);
// void RneFromLLA(const int32_t LLAi[3], float Rne[3][3]);
// ****** Express LLA in a local NED Base Frame and back ********
// void LLA2Base(const int32_t LLAi[3], const float BaseECEF[3], float Rne[3][3], float NED[3]);
// void Base2LLA(const float NED[3], const float BaseECEF[3], float Rne[3][3], int32_t LLAi[3]);
// ****** Express ECEF in a local NED Base Frame and back ********
// void ECEF2Base(const float ECEF[3], const float BaseECEF[3], float Rne[3][3], float NED[3]);
// void Base2ECEF(const float NED[3], const float BaseECEF[3], float Rne[3][3], float ECEF[3]
TEST_F(CoordinateConversionsTestRaw, LLA2ECEF) {
int32_t LLAi[3] = {
419291818,
125571688,
50 * 1e4
};
int32_t LLAfromECEF[3];
float ecef[3];
LLA2ECEF(LLAi, ecef);
ECEF2LLA(ecef, LLAfromECEF);
EXPECT_NEAR(LLAi[0], LLAfromECEF[0], epsilon_int_deg);
EXPECT_NEAR(LLAi[1], LLAfromECEF[1], epsilon_int_deg);
EXPECT_NEAR(LLAi[2], LLAfromECEF[2], epsilon_int_metric);
}
TEST_F(CoordinateConversionsTestRaw, LLA2NED) {
int32_t LLAi[3] = {
419291818,
125571688,
50 * 1e4
};
int32_t LLAfromNED[3];
int32_t HomeLLAi[3] = {
419291600,
125571300,
24 * 1e4
};
float Rne[3][3];
float baseECEF[3];
float NED[3];
RneFromLLA(HomeLLAi, Rne);
LLA2ECEF(HomeLLAi, baseECEF);
LLA2Base(LLAi, baseECEF, Rne, NED);
Base2LLA(NED, baseECEF, Rne, LLAfromNED);
EXPECT_NEAR(LLAi[0], LLAfromNED[0], epsilon_int_deg);
EXPECT_NEAR(LLAi[1], LLAfromNED[1], epsilon_int_deg);
EXPECT_NEAR(LLAi[2], LLAfromNED[2], epsilon_int_metric);
}

0
flight/tests/math/unittest.cpp → flight/tests/math/mathmishtest.cpp
View File

2
ground/gcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -59,6 +59,8 @@ UAVOBJS = \
$${UAVOBJ_XML_DIR}/auxmagsettings.xml \
$${UAVOBJ_XML_DIR}/barosensor.xml \
$${UAVOBJ_XML_DIR}/callbackinfo.xml \
$${UAVOBJ_XML_DIR}/cameracontrolactivity.xml \
$${UAVOBJ_XML_DIR}/cameracontrolsettings.xml \
$${UAVOBJ_XML_DIR}/cameradesired.xml \
$${UAVOBJ_XML_DIR}/camerastabsettings.xml \
$${UAVOBJ_XML_DIR}/debuglogcontrol.xml \

3
shared/uavobjectdefinition/callbackinfo.xml
View File

@ -12,6 +12,7 @@
<elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname>
<elementname>CameraControl</elementname>
<elementname>DebugLog</elementname>
</elementnames>
</field>
@ -26,6 +27,7 @@
<elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname>
<elementname>CameraControl</elementname>
<elementname>DebugLog</elementname>
</elementnames>
<options>
@ -44,6 +46,7 @@
<elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname>
<elementname>CameraControl</elementname>
<elementname>DebugLog</elementname>
</elementnames>
</field>

26
shared/uavobjectdefinition/cameracontrolactivity.xml Normal file
View File

@ -0,0 +1,26 @@
<xml>
<object name="CameraControlActivity" singleinstance="true" settings="false" category="State">
<description>Contains position and timestamp of each camera operation</description>
<field name="Latitude" units="degrees x 10^-7" type="int32" elements="1"/>
<field name="Longitude" units="degrees x 10^-7" type="int32" elements="1"/>
<field name="Altitude" units="m" type="float" elements="1"/>
<field name="Roll" units="degrees" type="float" elements="1"/>
<field name="Pitch" units="degrees" type="float" elements="1"/>
<field name="Yaw" units="degrees" type="float" elements="1"/>
<field name="ImageId" units="" type="uint16" elements="1"/>
<field name="TriggerMonth" units="" type="int8" elements="1"/>
<field name="TriggerDay" units="" type="int8" elements="1"/>
<field name="TriggerYear" units="" type="int16" elements="1"/>
<field name="TriggerHour" units="" type="int8" elements="1"/>
<field name="TriggerMinute" units="" type="int8" elements="1"/>
<field name="TriggerSecond" units="" type="int8" elements="1"/>
<field name="TriggerMillisecond" units="ms" type="int16" elements="1"/>
<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="SystemTS" units="ms" type="uint32" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

18
shared/uavobjectdefinition/cameracontrolsettings.xml Normal file
View File

@ -0,0 +1,18 @@
<xml>
<object name="CameraControlSettings" singleinstance="true" settings="true" category="Control">
<description>Settings for the @ref CameraControl module</description>
<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="TriggerPulseWidth" units="ms" type="uint16" elements="1" defaultvalue="50"/>
<field name="TimeInterval" units="s" type="float" elements="1" defaultvalue="5"/>
<field name="SpaceInterval" units="m" type="float" elements="1" defaultvalue="0"/>
<field name="MinimumTimeInterval" units="s" type="float" elements="1" defaultvalue="2.5"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

1
shared/uavobjectdefinition/cameradesired.xml
View File

@ -4,6 +4,7 @@
<field name="RollOrServo1" units="" type="float" elements="1"/>
<field name="PitchOrServo2" units="" type="float" elements="1"/>
<field name="Yaw" units="" type="float" elements="1"/>
<field name="Trigger" units="" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>

1
shared/uavobjectdefinition/gpstime.xml
View File

@ -7,6 +7,7 @@
<field name="Hour" units="" type="int8" elements="1"/>
<field name="Minute" units="" type="int8" elements="1"/>
<field name="Second" units="" type="int8" elements="1"/>
<field name="Millisecond" units="ms" type="int16" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="10000"/>

2
shared/uavobjectdefinition/hwsettings.xml
View File

@ -30,7 +30,7 @@
<field name="USB_HIDPort" units="function" type="enum" elements="1" options="USBTelemetry,RCTransmitter,Disabled" defaultvalue="USBTelemetry"/>
<field name="USB_VCPPort" units="function" type="enum" elements="1" options="USBTelemetry,ComBridge,DebugConsole,Disabled" defaultvalue="Disabled"/>
<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,Fault,Altitude,Airspeed,TxPID,Battery,Overo,MagBaro,OsdHk,AutoTune" options="Disabled,Enabled" defaultvalue="Disabled"/>
<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,CameraControl,GPS,Fault,Altitude,Airspeed,TxPID,Battery,Overo,MagBaro,OsdHk,AutoTune" options="Disabled,Enabled" defaultvalue="Disabled"/>
<field name="ADCRouting" units="" type="enum" elementnames="adc0,adc1,adc2,adc3,adc4,adc5,adc6,adc7" options="Disabled,BatteryVoltage,BatteryCurrent,AnalogAirspeed,Generic" defaultvalue="Disabled"/>
<field name="DSMxBind" units="" type="uint8" elements="1" defaultvalue="0"/>
<field name="WS2811LED_Out" units="" type="enum" elements="1" options="ServoOut1,ServoOut2,ServoOut3,ServoOut4,ServoOut5,ServoOut6,FlexiIOPin3,FlexiIOPin4,Disabled" defaultvalue="Disabled"

1
shared/uavobjectdefinition/mixersettings.xml
View File

@ -32,6 +32,7 @@
<option>CameraRollOrServo1</option>
<option>CameraPitchOrServo2</option>
<option>CameraYaw</option>
<option>CameraTrigger</option>
<option>Accessory0</option>
<option>Accessory1</option>
<option>Accessory2</option>