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

AHRS: Made an object setting to allow the downsampling rate to be changed on the

Browse Source 
fly (EKF rate up to a limit).  Also, now the algorithm selects if you are
indoor or outdoor as well as if you use a mag indoor (if you do set the z
variance higher than it calibrates to).

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1841 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-10-02 03:10:01 +00:00 committed by peabody124
parent 104dc165a9
commit 0c30101f54
16 changed files with 61 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
flight/AHRS/ahrs.c
View File

@ -41,6 +41,7 @@
#include "CoordinateConversions.h"
#include "ahrs_spi_comm.h"
#define MAX_OVERSAMPLING 50
// For debugging the raw sensors
//#define DUMP_RAW
//#define DUMP_FRIENDLY
@ -57,7 +58,7 @@ extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
extern float K[NUMX][NUMV]; // feedback gain matrix
#endif
volatile enum algorithms ahrs_algorithm;
volatile int8_t ahrs_algorithm;
/**
* @addtogroup AHRS_Structures Local Structres
@ -175,7 +176,7 @@ uint32_t counter_val;
* Public data. Used by both EKF and the sender
*/
//! Filter coefficients used in decimation. Limited order so filter can't run between samples
int16_t fir_coeffs[50];
int16_t fir_coeffs[MAX_OVERSAMPLING];
//! The oversampling rate, ekf is 2k / this
@ -339,7 +340,7 @@ for all data to be up to date before doing anything*/
downsample_data();
/******************** INS ALGORITHM **************************/
if (ahrs_algorithm == INSGPS_Algo) {
if (ahrs_algorithm != AHRSSETTINGS_ALGORITHM_SIMPLE) {
// format data for INS algo
gyro[0] = gyro_data.filtered.x;
@ -358,7 +359,7 @@ for all data to be up to date before doing anything*/
send_attitude(); // get message out quickly
INSCovariancePrediction(1 / (float)EKF_RATE);
if (gps_data.updated && gps_data.quality == 1) {
if (gps_data.updated && ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) {
// Compute velocity from Heading and groundspeed
vel[0] =
gps_data.groundspeed *
@ -384,7 +385,7 @@ for all data to be up to date before doing anything*/
gps_data.updated = false;
mag_data.updated = 0;
} else if (gps_data.quality != -1
} else if (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR
&& mag_data.updated == 1) {
float mag_var[3] = {mag_data.calibration.variance[1], mag_data.calibration.variance[0], mag_data.calibration.variance[2]};
INSSetMagVar(mag_var);
@ -397,7 +398,7 @@ for all data to be up to date before doing anything*/
vel[1] = 0;
vel[2] = 0;
if(mag_data.updated == 1) {
if((mag_data.updated == 1) && (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR)) {
float mag_var[3] = {10,10,10};
INSSetMagVar(mag_var);
MagVelBaroCorrection(mag,vel,altitude_data.altitude); // only trust mags if outdoors
@ -795,14 +796,7 @@ void gps_callback(AhrsObjHandle obj)
HomeLocationData home;
HomeLocationGet(&home);
if(home.Set == HOMELOCATION_SET_FALSE || home.Indoor == HOMELOCATION_INDOOR_TRUE) {
gps_data.NED[0] = 0;
gps_data.NED[1] = 0;
gps_data.NED[2] = 0;
gps_data.groundspeed = 0;
gps_data.heading = 0;
gps_data.quality = -1; // indicates indoor mode, high variance zeros update
gps_data.updated = true;
if(ahrs_algorithm != AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) {
return;
}
@ -837,12 +831,22 @@ void settings_callback(AhrsObjHandle obj)
AHRSSettingsData settings;
AHRSSettingsGet(&settings);
if(settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS)
{
ahrs_algorithm = INSGPS_Algo;
}else
{
ahrs_algorithm = SIMPLE_Algo;
ahrs_algorithm = settings.Algorithm;
if(settings.Downsampling != adc_oversampling) {
adc_oversampling = settings.Downsampling;
if(adc_oversampling > MAX_OVERSAMPLING) {
adc_oversampling = MAX_OVERSAMPLING;
settings.Downsampling = MAX_OVERSAMPLING;
AHRSSettingsSet(&settings);
}
AHRS_ADC_Config(adc_oversampling);
/* Use simple averaging filter for now */
for (int i = 0; i < adc_oversampling; i++)
fir_coeffs[i] = 1;
fir_coeffs[adc_oversampling] = adc_oversampling;
}
}

1
flight/OpenPilot/UAVObjects/ahrssettings.c
View File

@ -81,6 +81,7 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
UAVObjGetInstanceData(obj, instId, &data);
memset(&data, 0, sizeof(AHRSSettingsData));
data.Algorithm = 1;
data.Downsampling = 20;
data.UpdateRaw = 0;
data.UpdateFiltered = 1;
data.UpdatePeriod = 20;

1
flight/OpenPilot/UAVObjects/homelocation.c
View File

@ -81,7 +81,6 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
UAVObjGetInstanceData(obj, instId, &data);
memset(&data, 0, sizeof(HomeLocationData));
data.Set = 0;
data.Indoor = 0;
data.Latitude = 0;
data.Longitude = 0;
data.Altitude = 0;

6
flight/OpenPilot/UAVObjects/inc/ahrssettings.h
View File

@ -41,7 +41,7 @@
#define AHRSSETTINGS_H
// Object constants
#define AHRSSETTINGS_OBJID 1565605328U
#define AHRSSETTINGS_OBJID 4141274898U
#define AHRSSETTINGS_NAME "AHRSSettings"
#define AHRSSETTINGS_METANAME "AHRSSettingsMeta"
#define AHRSSETTINGS_ISSINGLEINST 1
@ -72,6 +72,7 @@
// Object data
typedef struct {
uint8_t Algorithm;
uint8_t Downsampling;
uint8_t UpdateRaw;
uint8_t UpdateFiltered;
int32_t UpdatePeriod;
@ -81,7 +82,8 @@ typedef struct {
// Field information
// Field Algorithm information
/* Enumeration options for field Algorithm */
typedef enum { AHRSSETTINGS_ALGORITHM_SIMPLE=0, AHRSSETTINGS_ALGORITHM_INSGPS=1 } AHRSSettingsAlgorithmOptions;
typedef enum { AHRSSETTINGS_ALGORITHM_SIMPLE=0, AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR_NOMAG=1, AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR=2, AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR=3 } AHRSSettingsAlgorithmOptions;
// Field Downsampling information
// Field UpdateRaw information
/* Enumeration options for field UpdateRaw */
typedef enum { AHRSSETTINGS_UPDATERAW_FALSE=0, AHRSSETTINGS_UPDATERAW_TRUE=1 } AHRSSettingsUpdateRawOptions;

6
flight/OpenPilot/UAVObjects/inc/homelocation.h
View File

@ -41,7 +41,7 @@
#define HOMELOCATION_H
// Object constants
#define HOMELOCATION_OBJID 316455980U
#define HOMELOCATION_OBJID 3590360786U
#define HOMELOCATION_NAME "HomeLocation"
#define HOMELOCATION_METANAME "HomeLocationMeta"
#define HOMELOCATION_ISSINGLEINST 1
@ -72,7 +72,6 @@
// Object data
typedef struct {
uint8_t Set;
uint8_t Indoor;
int32_t Latitude;
int32_t Longitude;
float Altitude;
@ -86,9 +85,6 @@ typedef struct {
// Field Set information
/* Enumeration options for field Set */
typedef enum { HOMELOCATION_SET_FALSE=0, HOMELOCATION_SET_TRUE=1 } HomeLocationSetOptions;
// Field Indoor information
/* Enumeration options for field Indoor */
typedef enum { HOMELOCATION_INDOOR_FALSE=0, HOMELOCATION_INDOOR_TRUE=1 } HomeLocationIndoorOptions;
// Field Latitude information
// Field Longitude information
// Field Altitude information

2
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -18,6 +18,7 @@
6519133C1256C52B0039C0A3 /* ahrs_spi_program_master.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs_spi_program_master.h; sourceTree = "<group>"; };
6519133D1256C52B0039C0A3 /* ahrs_spi_program_slave.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs_spi_program_slave.h; sourceTree = "<group>"; };
6519133E1256C52B0039C0A3 /* ahrs_spi_program.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs_spi_program.h; sourceTree = "<group>"; };
651913571256D0030039C0A3 /* ahrssettings.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrssettings.h; sourceTree = "<group>"; };
651CF9E5120B5D8300EEFD70 /* pios_usb_hid_desc.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_usb_hid_desc.c; sourceTree = "<group>"; };
651CF9E6120B5D8300EEFD70 /* pios_usb_hid_istr.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_usb_hid_istr.c; sourceTree = "<group>"; };
651CF9E7120B5D8300EEFD70 /* pios_usb_hid_prop.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_usb_hid_prop.c; sourceTree = "<group>"; };
@ -7224,6 +7225,7 @@
65E8EF8011EEA61E00BBF654 /* inc */ = {
isa = PBXGroup;
children = (
651913571256D0030039C0A3 /* ahrssettings.h */,
65E8EF8111EEA61E00BBF654 /* actuatorcommand.h */,
65E8EF8211EEA61E00BBF654 /* actuatordesired.h */,
65E8EF8311EEA61E00BBF654 /* actuatorsettings.h */,

17
ground/src/plugins/config/ahrs.ui
View File

@ -294,23 +294,6 @@ Disabled if there is no GPS fix.</string>
<string>Save Position</string>
</property>
</widget>
<widget class="QCheckBox" name="indoorFlight">
<property name="geometry">
<rect>
<x>380</x>
<y>400</y>
<width>201</width>
<height>22</height>
</rect>
</property>
<property name="toolTip">
<string>Check this box if you are flying indoors and have no GPS fix.
Only makes sense with the &quot;INSGPS&quot; algorithm.</string>
</property>
<property name="text">
<string>Indoor flight mode</string>
</property>
</widget>
<widget class="QPushButton" name="ahrsSettingsSaveRAM">
<property name="geometry">
<rect>

12
ground/src/plugins/config/configahrswidget.cpp
View File

@ -689,9 +689,6 @@ void ConfigAHRSWidget::ahrsSettingsRequest()
drawVariancesGraph();
obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("HomeLocation")));
field = obj->getField(QString("Indoor"));
if (field)
m_ahrs->indoorFlight->setChecked(field->getValue().toBool());
field = obj->getField(QString("Set"));
if (field)
m_ahrs->homeLocationSet->setEnabled(field->getValue().toBool());
@ -712,10 +709,6 @@ void ConfigAHRSWidget::enableHomeLocSave(UAVObject * obj)
if (field) {
m_ahrs->homeLocationSet->setEnabled(field->getValue().toBool());
}
// While we're at it, ensure the 'indoor' flag is consistent:
field = obj->getField(QString("Indoor"));
if (field)
m_ahrs->indoorFlight->setChecked(field->getValue().toBool());
}
@ -729,11 +722,6 @@ void ConfigAHRSWidget::ahrsSettingsSaveRAM()
field->setValue(m_ahrs->algorithm->currentText());
obj->updated();
obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("HomeLocation")));
field = obj->getField(QString("Indoor"));
if (m_ahrs->indoorFlight->isChecked())
field->setValue(QString("TRUE"));
else
field->setValue(QString("FALSE"));
field = obj->getField(QString("Set"));
if (m_ahrs->homeLocationSet->isChecked())
field->setValue(QString("TRUE"));

8
ground/src/plugins/uavobjects/ahrssettings.cpp
View File

@ -46,8 +46,13 @@ AHRSSettings::AHRSSettings(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAM
AlgorithmElemNames.append("0");
QStringList AlgorithmEnumOptions;
AlgorithmEnumOptions.append("SIMPLE");
AlgorithmEnumOptions.append("INSGPS");
AlgorithmEnumOptions.append("INSGPS_INDOOR_NOMAG");
AlgorithmEnumOptions.append("INSGPS_INDOOR");
AlgorithmEnumOptions.append("INSGPS_OUTDOOR");
fields.append( new UAVObjectField(QString("Algorithm"), QString(""), UAVObjectField::ENUM, AlgorithmElemNames, AlgorithmEnumOptions) );
QStringList DownsamplingElemNames;
DownsamplingElemNames.append("0");
fields.append( new UAVObjectField(QString("Downsampling"), QString(""), UAVObjectField::UINT8, DownsamplingElemNames, QStringList()) );
QStringList UpdateRawElemNames;
UpdateRawElemNames.append("0");
QStringList UpdateRawEnumOptions;
@ -97,6 +102,7 @@ UAVObject::Metadata AHRSSettings::getDefaultMetadata()
void AHRSSettings::setDefaultFieldValues()
{
data.Algorithm = 1;
data.Downsampling = 20;
data.UpdateRaw = 0;
data.UpdateFiltered = 1;
data.UpdatePeriod = 20;

6
ground/src/plugins/uavobjects/ahrssettings.h
View File

@ -44,6 +44,7 @@ public:
// Field structure
typedef struct {
quint8 Algorithm;
quint8 Downsampling;
quint8 UpdateRaw;
quint8 UpdateFiltered;
qint32 UpdatePeriod;
@ -53,7 +54,8 @@ public:
// Field information
// Field Algorithm information
/* Enumeration options for field Algorithm */
typedef enum { ALGORITHM_SIMPLE=0, ALGORITHM_INSGPS=1 } AlgorithmOptions;
typedef enum { ALGORITHM_SIMPLE=0, ALGORITHM_INSGPS_INDOOR_NOMAG=1, ALGORITHM_INSGPS_INDOOR=2, ALGORITHM_INSGPS_OUTDOOR=3 } AlgorithmOptions;
// Field Downsampling information
// Field UpdateRaw information
/* Enumeration options for field UpdateRaw */
typedef enum { UPDATERAW_FALSE=0, UPDATERAW_TRUE=1 } UpdateRawOptions;
@ -64,7 +66,7 @@ public:
// Constants
static const quint32 OBJID = 1565605328U;
static const quint32 OBJID = 4141274898U;
static const QString NAME;
static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 1;

16
ground/src/plugins/uavobjects/ahrssettings.py
View File

@ -46,7 +46,19 @@ _fields = [ \
],
{
'0' : 'SIMPLE',
'1' : 'INSGPS',
'1' : 'INSGPS_INDOOR_NOMAG',
'2' : 'INSGPS_INDOOR',
'3' : 'INSGPS_OUTDOOR',
}
),
uavobject.UAVObjectField(
'Downsampling',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
@ -88,7 +100,7 @@ _fields = [ \
class AHRSSettings(uavobject.UAVObject):
## Object constants
OBJID = 1565605328
OBJID = 4141274898
NAME = "AHRSSettings"
METANAME = "AHRSSettingsMeta"
ISSINGLEINST = 1

7
ground/src/plugins/uavobjects/homelocation.cpp
View File

@ -48,12 +48,6 @@ HomeLocation::HomeLocation(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAM
SetEnumOptions.append("FALSE");
SetEnumOptions.append("TRUE");
fields.append( new UAVObjectField(QString("Set"), QString(""), UAVObjectField::ENUM, SetElemNames, SetEnumOptions) );
QStringList IndoorElemNames;
IndoorElemNames.append("0");
QStringList IndoorEnumOptions;
IndoorEnumOptions.append("FALSE");
IndoorEnumOptions.append("TRUE");
fields.append( new UAVObjectField(QString("Indoor"), QString(""), UAVObjectField::ENUM, IndoorElemNames, IndoorEnumOptions) );
QStringList LatitudeElemNames;
LatitudeElemNames.append("0");
fields.append( new UAVObjectField(QString("Latitude"), QString("deg * 10e6"), UAVObjectField::INT32, LatitudeElemNames, QStringList()) );
@ -118,7 +112,6 @@ UAVObject::Metadata HomeLocation::getDefaultMetadata()
void HomeLocation::setDefaultFieldValues()
{
data.Set = 0;
data.Indoor = 0;
data.Latitude = 0;
data.Longitude = 0;
data.Altitude = 0;

6
ground/src/plugins/uavobjects/homelocation.h
View File

@ -44,7 +44,6 @@ public:
// Field structure
typedef struct {
quint8 Set;
quint8 Indoor;
qint32 Latitude;
qint32 Longitude;
float Altitude;
@ -58,9 +57,6 @@ public:
// Field Set information
/* Enumeration options for field Set */
typedef enum { SET_FALSE=0, SET_TRUE=1 } SetOptions;
// Field Indoor information
/* Enumeration options for field Indoor */
typedef enum { INDOOR_FALSE=0, INDOOR_TRUE=1 } IndoorOptions;
// Field Latitude information
// Field Longitude information
// Field Altitude information
@ -76,7 +72,7 @@ public:
// Constants
static const quint32 OBJID = 316455980U;
static const quint32 OBJID = 3590360786U;
static const QString NAME;
static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 1;

14
ground/src/plugins/uavobjects/homelocation.py
View File

@ -49,18 +49,6 @@ _fields = [ \
'1' : 'TRUE',
}
),
uavobject.UAVObjectField(
'Indoor',
'b',
1,
[
'0',
],
{
'0' : 'FALSE',
'1' : 'TRUE',
}
),
uavobject.UAVObjectField(
'Latitude',
'i',
@ -138,7 +126,7 @@ _fields = [ \
class HomeLocation(uavobject.UAVObject):
## Object constants
OBJID = 316455980
OBJID = 3590360786
NAME = "HomeLocation"
METANAME = "HomeLocationMeta"
ISSINGLEINST = 1

3
ground/src/shared/uavobjectdefinition/ahrssettings.xml
View File

@ -1,7 +1,8 @@
<xml>
<object name="AHRSSettings" singleinstance="true" settings="true">
<description>Settings for the @ref AHRSCommsModule to control the algorithm and what is updated</description>
<field name="Algorithm" units="" type="enum" elements="1" options="SIMPLE,INSGPS" defaultvalue="INSGPS"/>
<field name="Algorithm" units="" type="enum" elements="1" options="SIMPLE,INSGPS_INDOOR_NOMAG,INSGPS_INDOOR,INSGPS_OUTDOOR" defaultvalue="INSGPS_INDOOR_NOMAG"/>
<field name="Downsampling" units="" type="uint8" elements="1" defaultvalue="20"/>
<field name="UpdateRaw" units="raw" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="UpdateFiltered" units="raw" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
<field name="UpdatePeriod" units="ms" type="int32" elements="1" defaultvalue="20"/>

1
ground/src/shared/uavobjectdefinition/homelocation.xml
View File

@ -2,7 +2,6 @@
<object name="HomeLocation" singleinstance="true" settings="true">
<description>HomeLocation setting which contains the constants to tranlate from longitutde and latitude to NED reference frame. Automatically set by @ref GPSModule after acquiring a 3D lock. Used by @ref AHRSCommsModule.</description>
<field name="Set" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="Indoor" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="Latitude" units="deg * 10e6" type="int32" elements="1" defaultvalue="0"/>
<field name="Longitude" units="deg * 10e6" type="int32" elements="1" defaultvalue="0"/>
<field name="Altitude" units="m over geoid" type="float" elements="1" defaultvalue="0"/>