rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

Merge branch 'corvuscorax/lla2nedfilter' into next

Browse Source 
Conflicts:
	flight/modules/StateEstimation/stateestimation.c
This commit is contained in:
Corvus Corax 2013-07-10 14:38:12 +02:00
commit 27eadc200b
17 changed files with 231 additions and 122 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
flight/libraries/CoordinateConversions.c
View File

@ -35,23 +35,29 @@
#define MIN_ALLOWABLE_MAGNITUDE 1e-30f
// ****** convert Lat,Lon,Alt to ECEF ************
void LLA2ECEF(float LLA[3], double ECEF[3])
void LLA2ECEF(int32_t LLAi[3], double ECEF[3])
{
const double a = 6378137.0d; // Equatorial Radius
const double e = 8.1819190842622e-2d; // Eccentricity
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
};
sinLat = sin(DEG2RAD(LLA[0]));
sinLon = sin(DEG2RAD(LLA[1]));
cosLat = cos(DEG2RAD(LLA[0]));
cosLon = cos(DEG2RAD(LLA[1]));
sinLat = sin(DEG2RAD_D(LLA[0]));
sinLon = sin(DEG2RAD_D(LLA[1]));
cosLat = cos(DEG2RAD_D(LLA[0]));
cosLon = cos(DEG2RAD_D(LLA[1]));
N = a / sqrt(1.0d - e * e * sinLat * sinLat); // prime vertical radius of curvature
N = a / sqrt(1.0d - e2 * sinLat * sinLat); // prime vertical radius of curvature
ECEF[0] = (N + (double)LLA[2]) * cosLat * cosLon;
ECEF[1] = (N + (double)LLA[2]) * cosLat * sinLon;
ECEF[2] = ((1 - e * e) * N + (double)LLA[2]) * sinLat;
ECEF[0] = (N + LLA[2]) * cosLat * cosLon;
ECEF[1] = (N + LLA[2]) * cosLat * sinLon;
ECEF[2] = ((1.0d - e2) * N + LLA[2]) * sinLat;
}
// ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
@ -100,14 +106,14 @@ uint16_t ECEF2LLA(double ECEF[3], float LLA[3])
}
// ****** find ECEF to NED rotation matrix ********
void RneFromLLA(float LLA[3], double Rne[3][3])
void RneFromLLA(int32_t LLAi[3], float Rne[3][3])
{
double sinLat, sinLon, cosLat, cosLon;
float sinLat, sinLon, cosLat, cosLon;
sinLat = sin(DEG2RAD(LLA[0]));
sinLon = sin(DEG2RAD(LLA[1]));
cosLat = cos(DEG2RAD(LLA[0]));
cosLon = cos(DEG2RAD(LLA[1]));
sinLat = sinf(DEG2RAD((float)LLAi[0] * 1e-7f));
sinLon = sinf(DEG2RAD((float)LLAi[1] * 1e-7f));
cosLat = cosf(DEG2RAD((float)LLAi[0] * 1e-7f));
cosLon = cosf(DEG2RAD((float)LLAi[1] * 1e-7f));
Rne[0][0] = -sinLat * cosLon;
Rne[0][1] = -sinLat * sinLon;
@ -188,16 +194,16 @@ void Quaternion2R(float q[4], float Rbe[3][3])
}
// ****** Express LLA in a local NED Base Frame ********
void LLA2Base(float LLA[3], double BaseECEF[3], double Rne[3][3], float NED[3])
void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3])
{
double ECEF[3];
double diff[3];
float diff[3];
LLA2ECEF(LLA, ECEF);
LLA2ECEF(LLAi, ECEF);
diff[0] = (ECEF[0] - BaseECEF[0]);
diff[1] = (ECEF[1] - BaseECEF[1]);
diff[2] = (ECEF[2] - BaseECEF[2]);
diff[0] = (float)(ECEF[0] - BaseECEF[0]);
diff[1] = (float)(ECEF[1] - BaseECEF[1]);
diff[2] = (float)(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];
@ -205,13 +211,13 @@ void LLA2Base(float LLA[3], double BaseECEF[3], double Rne[3][3], float NED[3])
}
// ****** Express ECEF in a local NED Base Frame ********
void ECEF2Base(double ECEF[3], double BaseECEF[3], double Rne[3][3], float NED[3])
void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3])
{
double diff[3];
float diff[3];
diff[0] = (ECEF[0] - BaseECEF[0]);
diff[1] = (ECEF[1] - BaseECEF[1]);
diff[2] = (ECEF[2] - BaseECEF[2]);
diff[0] = (float)(ECEF[0] - BaseECEF[0]);
diff[1] = (float)(ECEF[1] - BaseECEF[1]);
diff[2] = (float)(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];

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

@ -31,12 +31,12 @@
#define COORDINATECONVERSIONS_H_
// ****** convert Lat,Lon,Alt to ECEF ************
void LLA2ECEF(float LLA[3], double ECEF[3]);
void LLA2ECEF(int32_t LLAi[3], double ECEF[3]);
// ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
uint16_t ECEF2LLA(double ECEF[3], float LLA[3]);
void RneFromLLA(float LLA[3], double Rne[3][3]);
void RneFromLLA(int32_t LLAi[3], float Rne[3][3]);
// ****** find rotation matrix from rotation vector
void Rv2Rot(float Rv[3], float R[3][3]);
@ -51,10 +51,10 @@ void RPY2Quaternion(const float rpy[3], float q[4]);
void Quaternion2R(float q[4], float Rbe[3][3]);
// ****** Express LLA in a local NED Base Frame ********
void LLA2Base(float LLA[3], double BaseECEF[3], double Rne[3][3], float NED[3]);
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], double Rne[3][3], float NED[3]);
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 ****

61
flight/modules/GPS/GPS.c
View File

@ -34,11 +34,10 @@
#include "gpspositionsensor.h"
#include "homelocation.h"
#include "positionsensor.h"
#include "gpstime.h"
#include "gpssatellites.h"
#include "gpsvelocitysensor.h"
#include "systemsettings.h"
#include "gpssettings.h"
#include "taskinfo.h"
#include "hwsettings.h"
@ -60,9 +59,6 @@ static void updateSettings();
static void setHomeLocation(GPSPositionSensorData *gpsData);
static float GravityAccel(float latitude, float longitude, float altitude);
#endif
#ifdef PIOS_GPS_SETS_POSITIONSENSOR
static void setPositionSensor(GPSPositionSensorData *gpsData);
#endif
// ****************
// Private constants
@ -154,7 +150,6 @@ int32_t GPSInitialize(void)
GPSTimeInitialize();
GPSSatellitesInitialize();
HomeLocationInitialize();
PositionSensorInitialize();
updateSettings();
#else
@ -165,24 +160,21 @@ int32_t GPSInitialize(void)
GPSTimeInitialize();
GPSSatellitesInitialize();
#endif
#if (defined(PIOS_GPS_SETS_HOMELOCATION) || defined(PIOS_GPS_SETS_POSITIONSENSOR))
#if defined(PIOS_GPS_SETS_HOMELOCATION)
HomeLocationInitialize();
#endif
#ifdef PIOS_GPS_SETS_POSITIONSENSOR
PositionSensorInitialize();
#endif
updateSettings();
}
#endif /* if defined(REVOLUTION) */
if (gpsPort && gpsEnabled) {
SystemSettingsInitialize();
SystemSettingsGPSDataProtocolGet(&gpsProtocol);
GPSSettingsInitialize();
GPSSettingsDataProtocolGet(&gpsProtocol);
switch (gpsProtocol) {
case SYSTEMSETTINGS_GPSDATAPROTOCOL_NMEA:
case GPSSETTINGS_DATAPROTOCOL_NMEA:
gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH);
break;
case SYSTEMSETTINGS_GPSDATAPROTOCOL_UBX:
case GPSSETTINGS_DATAPROTOCOL_UBX:
gps_rx_buffer = pvPortMalloc(sizeof(struct UBXPacket));
break;
default:
@ -209,9 +201,9 @@ static void gpsTask(__attribute__((unused)) void *parameters)
uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
GPSPositionSensorData gpspositionsensor;
uint8_t gpsProtocol;
GPSSettingsData gpsSettings;
SystemSettingsGPSDataProtocolGet(&gpsProtocol);
GPSSettingsGet(&gpsSettings);
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
@ -224,14 +216,14 @@ static void gpsTask(__attribute__((unused)) void *parameters)
// This blocks the task until there is something on the buffer
while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) {
int res;
switch (gpsProtocol) {
switch (gpsSettings.DataProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
case SYSTEMSETTINGS_GPSDATAPROTOCOL_NMEA:
case GPSSETTINGS_DATAPROTOCOL_NMEA:
res = parse_nmea_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
break;
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
case SYSTEMSETTINGS_GPSDATAPROTOCOL_UBX:
case GPSSETTINGS_DATAPROTOCOL_UBX:
res = parse_ubx_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
break;
#endif
@ -259,7 +251,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
// we appear to be receiving GPS sentences OK, we've had an update
// criteria for GPS-OK taken from this post...
// http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
if ((gpspositionsensor.PDOP < 3.5f) && (gpspositionsensor.Satellites >= 7) &&
if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSattelites) &&
(gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
(gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
@ -270,9 +262,6 @@ static void gpsTask(__attribute__((unused)) void *parameters)
if (home.Set == HOMELOCATION_SET_FALSE) {
setHomeLocation(&gpspositionsensor);
}
#endif
#ifdef PIOS_GPS_SETS_POSITIONSENSOR
setPositionSensor(&gpspositionsensor);
#endif
} else if ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
(gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
@ -336,32 +325,6 @@ static void setHomeLocation(GPSPositionSensorData *gpsData)
}
#endif /* ifdef PIOS_GPS_SETS_HOMELOCATION */
#ifdef PIOS_GPS_SETS_POSITIONSENSOR
static void setPositionSensor(GPSPositionSensorData *gpsData)
{
HomeLocationData home;
HomeLocationGet(&home);
PositionSensorData pos;
PositionSensorGet(&pos);
double ECEF[3];
double Rne[3][3];
{
float LLA[3] = { (home.Latitude) / 10e6f, (home.Longitude) / 10e6f, (home.Altitude) };
LLA2ECEF(LLA, ECEF);
RneFromLLA(LLA, Rne);
}
{ float LLA[3] = { (gpsData->Latitude) / 10e6d, (gpsData->Longitude) / 10e6d, gpsData->Altitude + gpsData->GeoidSeparation };
float NED[3];
LLA2Base(LLA, ECEF, Rne, NED);
pos.North = NED[0];
pos.East = NED[1];
pos.Down = NED[2]; }
PositionSensorSet(&pos);
}
#endif /* ifdef PIOS_GPS_SETS_POSITIONSENSOR */
/**
* Update the GPS settings, called on startup.
* FIXME: This should be in the GPSSettings object. But objects have

125
flight/modules/StateEstimation/filterlla.c Normal file
View File

@ -0,0 +1,125 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup State Estimation
* @brief Acquires sensor data and computes state estimate
* @{
*
* @file filterlla.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
* @brief Computes NED position from GPS LLA data
*
* @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
*/
#include "inc/stateestimation.h"
#include <CoordinateConversions.h>
#include <homelocation.h>
#include <gpssettings.h>
#include <gpspositionsensor.h>
// Private constants
#define STACK_REQUIRED 256
// Private types
struct data {
GPSSettingsData settings;
HomeLocationData home;
double HomeECEF[3];
float HomeRne[3][3];
};
// Private variables
// Private functions
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
int32_t filterLLAInitialize(stateFilter *handle)
{
handle->init = &init;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
GPSSettingsInitialize();
GPSPositionSensorInitialize();
HomeLocationInitialize();
return STACK_REQUIRED;
}
static int32_t init(__attribute__((unused)) stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
GPSSettingsGet(&this->settings);
HomeLocationGet(&this->home);
if (this->home.Set == HOMELOCATION_SET_TRUE) {
// calculate home location coordinate reference
int32_t LLAi[3] = {
this->home.Latitude,
this->home.Longitude,
(int32_t)(this->home.Altitude * 1e4f),
};
LLA2ECEF(LLAi, this->HomeECEF);
RneFromLLA(LLAi, this->HomeRne);
}
return 0;
}
static int32_t filter(__attribute__((unused)) stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
// cannot update local NED if home location is unset
if (this->home.Set != HOMELOCATION_SET_TRUE) {
return 0;
}
// only do stuff if we have a valid GPS update
if (IS_SET(state->updated, SENSORUPDATES_lla)) {
// LLA information is not part of the state blob, due to its non standard layout (fixed point representation, quality of signal, ...)
// this filter deals with the gory details of interpreting it and storing it in a standard Cartesian position state
GPSPositionSensorData gpsdata;
GPSPositionSensorGet(&gpsdata);
// check if we have a valid GPS signal (not checked by StateEstimation istelf)
if ((gpsdata.PDOP < this->settings.MaxPDOP) && (gpsdata.Satellites >= this->settings.MinSattelites) &&
(gpsdata.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
(gpsdata.Latitude != 0 || gpsdata.Longitude != 0)) {
int32_t LLAi[3] = {
gpsdata.Latitude,
gpsdata.Longitude,
(int32_t)((gpsdata.Altitude + gpsdata.GeoidSeparation) * 1e4f),
};
LLA2Base(LLAi, this->HomeECEF, this->HomeRne, state->pos);
state->updated |= SENSORUPDATES_pos;
}
}
return 0;
}
/**
* @}
* @}
*/

2
flight/modules/StateEstimation/inc/stateestimation.h
View File

@ -41,6 +41,7 @@ typedef enum {
SENSORUPDATES_vel = 1 << 5,
SENSORUPDATES_airspeed = 1 << 6,
SENSORUPDATES_baro = 1 << 7,
SENSORUPDATES_lla = 1 << 8,
} sensorUpdates;
typedef struct {
@ -67,6 +68,7 @@ int32_t filterBaroInitialize(stateFilter *handle);
int32_t filterAltitudeInitialize(stateFilter *handle);
int32_t filterAirInitialize(stateFilter *handle);
int32_t filterStationaryInitialize(stateFilter *handle);
int32_t filterLLAInitialize(stateFilter *handle);
int32_t filterCFInitialize(stateFilter *handle);
int32_t filterCFMInitialize(stateFilter *handle);
int32_t filterEKF13iInitialize(stateFilter *handle);

56
flight/modules/StateEstimation/stateestimation.c
View File

@ -36,7 +36,7 @@
#include <magsensor.h>
#include <barosensor.h>
#include <airspeedsensor.h>
#include <positionsensor.h>
#include <gpspositionsensor.h>
#include <gpsvelocitysensor.h>
#include <gyrostate.h>
@ -126,6 +126,7 @@ static stateFilter baroFilter;
static stateFilter altitudeFilter;
static stateFilter airFilter;
static stateFilter stationaryFilter;
static stateFilter llaFilter;
static stateFilter cfFilter;
static stateFilter cfmFilter;
static stateFilter ekf13iFilter;
@ -137,12 +138,15 @@ static filterPipeline *cfQueue = &(filterPipeline) {
.next = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &baroFilter,
.filter = &llaFilter,
.next = &(filterPipeline) {
.filter = &altitudeFilter,
.filter = &baroFilter,
.next = &(filterPipeline) {
.filter = &cfFilter,
.next = NULL,
.filter = &altitudeFilter,
.next = &(filterPipeline) {
.filter = &cfFilter,
.next = NULL,
}
}
}
}
@ -153,12 +157,15 @@ static const filterPipeline *cfmQueue = &(filterPipeline) {
.next = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &baroFilter,
.filter = &llaFilter,
.next = &(filterPipeline) {
.filter = &altitudeFilter,
.filter = &baroFilter,
.next = &(filterPipeline) {
.filter = &cfmFilter,
.next = NULL,
.filter = &altitudeFilter,
.next = &(filterPipeline) {
.filter = &cfmFilter,
.next = NULL,
}
}
}
}
@ -169,12 +176,15 @@ static const filterPipeline *ekf13iQueue = &(filterPipeline) {
.next = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &baroFilter,
.filter = &llaFilter,
.next = &(filterPipeline) {
.filter = &stationaryFilter,
.filter = &baroFilter,
.next = &(filterPipeline) {
.filter = &ekf13iFilter,
.next = NULL,
.filter = &stationaryFilter,
.next = &(filterPipeline) {
.filter = &ekf13iFilter,
.next = NULL,
}
}
}
}
@ -185,10 +195,13 @@ static const filterPipeline *ekf13Queue = &(filterPipeline) {
.next = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &baroFilter,
.filter = &llaFilter,
.next = &(filterPipeline) {
.filter = &ekf13Filter,
.next = NULL,
.filter = &baroFilter,
.next = &(filterPipeline) {
.filter = &ekf13Filter,
.next = NULL,
}
}
}
}
@ -220,8 +233,8 @@ int32_t StateEstimationInitialize(void)
MagSensorInitialize();
BaroSensorInitialize();
AirspeedSensorInitialize();
PositionSensorInitialize();
GPSVelocitySensorInitialize();
GPSPositionSensorInitialize();
GyroStateInitialize();
AccelStateInitialize();
@ -237,8 +250,8 @@ int32_t StateEstimationInitialize(void)
MagSensorConnectCallback(&sensorUpdatedCb);
BaroSensorConnectCallback(&sensorUpdatedCb);
AirspeedSensorConnectCallback(&sensorUpdatedCb);
PositionSensorConnectCallback(&sensorUpdatedCb);
GPSVelocitySensorConnectCallback(&sensorUpdatedCb);
GPSPositionSensorConnectCallback(&sensorUpdatedCb);
uint32_t stack_required = STACK_SIZE_BYTES;
// Initialize Filters
@ -247,6 +260,7 @@ int32_t StateEstimationInitialize(void)
stack_required = maxint32_t(stack_required, filterAltitudeInitialize(&altitudeFilter));
stack_required = maxint32_t(stack_required, filterAirInitialize(&airFilter));
stack_required = maxint32_t(stack_required, filterStationaryInitialize(&stationaryFilter));
stack_required = maxint32_t(stack_required, filterLLAInitialize(&llaFilter));
stack_required = maxint32_t(stack_required, filterCFInitialize(&cfFilter));
stack_required = maxint32_t(stack_required, filterCFMInitialize(&cfmFilter));
stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter));
@ -361,11 +375,11 @@ static void StateEstimationCb(void)
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, mag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(PositionSensor, pos, North, East, Down);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
states.airspeed[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter, set to zero for now
// GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself
// at this point sensor state is stored in "states" with some rudimentary filtering applied
@ -483,8 +497,8 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
updatedSensors |= SENSORUPDATES_mag;
}
if (ev->obj == PositionSensorHandle()) {
updatedSensors |= SENSORUPDATES_pos;
if (ev->obj == GPSPositionSensorHandle()) {
updatedSensors |= SENSORUPDATES_lla;
}
if (ev->obj == GPSVelocitySensorHandle()) {

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

@ -101,6 +101,7 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/cameradesired.c
SRC += $(OPUAVSYNTHDIR)/gpspositionsensor.c
SRC += $(OPUAVSYNTHDIR)/gpsvelocitysensor.c
SRC += $(OPUAVSYNTHDIR)/gpssettings.c
SRC += $(OPUAVSYNTHDIR)/hwsettings.c
SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c
SRC += $(OPUAVSYNTHDIR)/receiveractivity.c

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

@ -91,6 +91,7 @@ ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/gpssatellites.c
SRC += $(OPUAVSYNTHDIR)/gpsvelocitysensor.c
SRC += $(OPUAVSYNTHDIR)/gpstime.c
SRC += $(OPUAVSYNTHDIR)/gpssettings.c
SRC += $(OPUAVSYNTHDIR)/osdsettings.c
SRC += $(OPUAVSYNTHDIR)/barosensor.c
SRC += $(OPUAVSYNTHDIR)/magsensor.c

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

@ -49,6 +49,7 @@ UAVOBJSRCFILENAMES += gpspositionsensor
UAVOBJSRCFILENAMES += gpssatellites
UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += gpsvelocitysensor
UAVOBJSRCFILENAMES += gpssettings
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += vtolpathfollowersettings
@ -66,7 +67,6 @@ UAVOBJSRCFILENAMES += overosyncsettings
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionsensor
UAVOBJSRCFILENAMES += positionstate
UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += relaytuning

1
flight/targets/boards/revolution/firmware/inc/pios_config.h
View File

@ -137,7 +137,6 @@
#define PIOS_INCLUDE_GPS_NMEA_PARSER
#define PIOS_INCLUDE_GPS_UBX_PARSER
#define PIOS_GPS_SETS_HOMELOCATION
#define PIOS_GPS_SETS_POSITIONSENSOR
/* Stabilization options */
/* #define PIOS_QUATERNION_STABILIZATION */

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

@ -54,6 +54,7 @@ UAVOBJSRCFILENAMES += gpspositionsensor
UAVOBJSRCFILENAMES += gpssatellites
UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += gpsvelocitysensor
UAVOBJSRCFILENAMES += gpssettings
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += vtolpathfollowersettings
@ -70,7 +71,6 @@ UAVOBJSRCFILENAMES += overosyncsettings
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionsensor
UAVOBJSRCFILENAMES += positionstate
UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += relaytuning

1
flight/targets/boards/revoproto/firmware/inc/pios_config.h
View File

@ -137,7 +137,6 @@
#define PIOS_INCLUDE_GPS_NMEA_PARSER
#define PIOS_INCLUDE_GPS_UBX_PARSER
#define PIOS_GPS_SETS_HOMELOCATION
#define PIOS_GPS_SETS_POSITIONSENSOR
/* Stabilization options */
/* #define PIOS_QUATERNION_STABILIZATION */

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

@ -54,6 +54,7 @@ UAVOBJSRCFILENAMES += gpspositionsensor
UAVOBJSRCFILENAMES += gpssatellites
UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += gpsvelocitysensor
UAVOBJSRCFILENAMES += gpssettings
UAVOBJSRCFILENAMES += vtolpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
@ -69,7 +70,6 @@ UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionsensor
UAVOBJSRCFILENAMES += positionstate
UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += revocalibration

4
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -63,11 +63,11 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/gpspositionsensor.h \
$$UAVOBJECT_SYNTHETICS/gpstime.h \
$$UAVOBJECT_SYNTHETICS/gpssatellites.h \
$$UAVOBJECT_SYNTHETICS/gpssettings.h \
$$UAVOBJECT_SYNTHETICS/pathaction.h \
$$UAVOBJECT_SYNTHETICS/pathdesired.h \
$$UAVOBJECT_SYNTHETICS/pathstatus.h \
$$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.h \
$$UAVOBJECT_SYNTHETICS/positionsensor.h \
$$UAVOBJECT_SYNTHETICS/positionstate.h \
$$UAVOBJECT_SYNTHETICS/flightbatterystate.h \
$$UAVOBJECT_SYNTHETICS/homelocation.h \
@ -150,11 +150,11 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/gpspositionsensor.cpp \
$$UAVOBJECT_SYNTHETICS/gpstime.cpp \
$$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \
$$UAVOBJECT_SYNTHETICS/gpssettings.cpp \
$$UAVOBJECT_SYNTHETICS/pathaction.cpp \
$$UAVOBJECT_SYNTHETICS/pathdesired.cpp \
$$UAVOBJECT_SYNTHETICS/pathstatus.cpp \
$$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.cpp \
$$UAVOBJECT_SYNTHETICS/positionsensor.cpp \
$$UAVOBJECT_SYNTHETICS/positionstate.cpp \
$$UAVOBJECT_SYNTHETICS/flightbatterystate.cpp \
$$UAVOBJECT_SYNTHETICS/homelocation.cpp \

12
shared/uavobjectdefinition/gpssettings.xml Normal file
View File

@ -0,0 +1,12 @@
<xml>
<object name="GPSSettings" singleinstance="true" settings="true" category="Sensors">
<description>GPS Module specific settings</description>
<field name="DataProtocol" units="" type="enum" elements="1" options="NMEA,UBX" defaultvalue="UBX"/>
<field name="MinSattelites" units="" type="uint8" elements="1" defaultvalue="7"/>
<field name="MaxPDOP" units="" type="float" elements="1" defaultvalue="3.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>

12
shared/uavobjectdefinition/positionsensor.xml
View File

@ -1,12 +0,0 @@
<xml>
<object name="PositionSensor" singleinstance="true" settings="false" category="Sensors">
<description>Contains the position in NED frame measured relative to @ref HomeLocation.</description>
<field name="North" units="m" type="float" elements="1"/>
<field name="East" units="m" type="float" elements="1"/>
<field name="Down" units="m" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="periodic" period="1000"/>
</object>
</xml>

1
shared/uavobjectdefinition/systemsettings.xml
View File

@ -3,7 +3,6 @@
<description>Select airframe type. Currently used by @ref ActuatorModule to choose mixing from @ref ActuatorDesired to @ref ActuatorCommand</description>
<field name="AirframeType" units="" type="enum" elements="1" options="FixedWing,FixedWingElevon,FixedWingVtail,VTOL,HeliCP,QuadX,QuadP,Hexa,Octo,Custom,HexaX,OctoV,OctoCoaxP,OctoCoaxX,HexaCoax,Tri,GroundVehicleCar,GroundVehicleDifferential,GroundVehicleMotorcycle" defaultvalue="QuadX"/>
<field name="GUIConfigData" units="bits" type="uint32" elements="4" defaultvalue="0"/>
<field name="GPSDataProtocol" units="" type="enum" elements="1" options="NMEA,UBX" defaultvalue="UBX"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>