rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

AHRS: Add a configuration to export the sensor data either with or without INS

Browse Source 
bias correction.  This is useful for stabilization to get rid of the gyro bias.
Making it an option is important for calibration.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2189 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-12-04 17:34:25 +00:00 committed by peabody124
parent 81b2f88e75
commit 6f1e7b4e41
8 changed files with 58 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
flight/AHRS/ahrs.c
View File

@ -638,9 +638,9 @@ void downsample_data()
raw.gyrotemp[0] = valid_data_buffer[6];
raw.gyrotemp[1] = valid_data_buffer[7];
raw.gyros_filtered[0] = (gyro_data.filtered.x - Nav.gyro_bias[0]) * 180 / M_PI;
raw.gyros_filtered[1] = (gyro_data.filtered.y - Nav.gyro_bias[1]) * 180 / M_PI;
raw.gyros_filtered[2] = (gyro_data.filtered.z - Nav.gyro_bias[2]) * 180 / M_PI;
raw.gyros_filtered[0] = gyro_data.filtered.x * 180 / M_PI;
raw.gyros_filtered[1] = gyro_data.filtered.y * 180 / M_PI;
raw.gyros_filtered[2] = gyro_data.filtered.z * 180 / M_PI;
raw.accels[0] = valid_data_buffer[2];
raw.accels[1] = valid_data_buffer[0];
@ -654,6 +654,17 @@ void downsample_data()
raw.magnetometers[1] = mag_data.scaled.axis[1];
raw.magnetometers[2] = mag_data.scaled.axis[2];
AHRSSettingsData settings;
AHRSSettingsGet(&settings);
if(settings.BiasCorrectedRaw == AHRSSETTINGS_BIASCORRECTEDRAW_TRUE) {
raw.gyros_filtered[0] -= Nav.gyro_bias[0] * 180 / M_PI;
raw.gyros_filtered[1] -= Nav.gyro_bias[1] * 180 / M_PI;
raw.gyros_filtered[2] -= Nav.gyro_bias[2] * 180 / M_PI;
raw.accels_filtered[0] -= Nav.accel_bias[0];
raw.accels_filtered[1] -= Nav.accel_bias[1];
raw.accels_filtered[2] -= Nav.accel_bias[2];
}
AttitudeRawSet(&raw);
}

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

@ -83,6 +83,7 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
data.Algorithm = 1;
data.Downsampling = 20;
data.UpdatePeriod = 1;
data.BiasCorrectedRaw = 0;
data.YawBias = 0;
data.PitchBias = 0;
data.RollBias = 0;

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

@ -41,7 +41,7 @@
#define AHRSSETTINGS_H
// Object constants
#define AHRSSETTINGS_OBJID 1456050280U
#define AHRSSETTINGS_OBJID 3741078856U
#define AHRSSETTINGS_NAME "AHRSSettings"
#define AHRSSETTINGS_METANAME "AHRSSettingsMeta"
#define AHRSSETTINGS_ISSINGLEINST 1
@ -74,6 +74,7 @@ typedef struct {
uint8_t Algorithm;
uint8_t Downsampling;
uint8_t UpdatePeriod;
uint8_t BiasCorrectedRaw;
float YawBias;
float PitchBias;
float RollBias;
@ -86,6 +87,9 @@ typedef struct {
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 UpdatePeriod information
// Field BiasCorrectedRaw information
/* Enumeration options for field BiasCorrectedRaw */
typedef enum { AHRSSETTINGS_BIASCORRECTEDRAW_TRUE=0, AHRSSETTINGS_BIASCORRECTEDRAW_FALSE=1 } AHRSSettingsBiasCorrectedRawOptions;
// Field YawBias information
// Field PitchBias information
// Field RollBias information

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

@ -557,6 +557,12 @@ void ConfigAHRSWidget::computeScaleBias()
obj->updated();
obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings")));
field = obj->getField(QString("BiasCorrectedRaw"));
field->setValue("TRUE");
obj->updated();
position = -1; //set to run again
m_ahrs->sixPointCalibInstructions->append("Computed accel and mag scale and bias...");
@ -597,6 +603,12 @@ void ConfigAHRSWidget::sixPointCalibrationMode()
obj->updated();
obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings")));
field = obj->getField(QString("BiasCorrectedRaw"));
field->setValue("FALSE");
obj->updated();
Thread::usleep(100000);
gyro_accum_x.clear();

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

@ -56,6 +56,12 @@ AHRSSettings::AHRSSettings(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAM
QStringList UpdatePeriodElemNames;
UpdatePeriodElemNames.append("0");
fields.append( new UAVObjectField(QString("UpdatePeriod"), QString("ms"), UAVObjectField::UINT8, UpdatePeriodElemNames, QStringList()) );
QStringList BiasCorrectedRawElemNames;
BiasCorrectedRawElemNames.append("0");
QStringList BiasCorrectedRawEnumOptions;
BiasCorrectedRawEnumOptions.append("TRUE");
BiasCorrectedRawEnumOptions.append("FALSE");
fields.append( new UAVObjectField(QString("BiasCorrectedRaw"), QString(""), UAVObjectField::ENUM, BiasCorrectedRawElemNames, BiasCorrectedRawEnumOptions) );
QStringList YawBiasElemNames;
YawBiasElemNames.append("0");
fields.append( new UAVObjectField(QString("YawBias"), QString(""), UAVObjectField::FLOAT32, YawBiasElemNames, QStringList()) );
@ -101,6 +107,7 @@ void AHRSSettings::setDefaultFieldValues()
data.Algorithm = 1;
data.Downsampling = 20;
data.UpdatePeriod = 1;
data.BiasCorrectedRaw = 0;
data.YawBias = 0;
data.PitchBias = 0;
data.RollBias = 0;

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

@ -46,6 +46,7 @@ public:
quint8 Algorithm;
quint8 Downsampling;
quint8 UpdatePeriod;
quint8 BiasCorrectedRaw;
float YawBias;
float PitchBias;
float RollBias;
@ -58,13 +59,16 @@ public:
typedef enum { ALGORITHM_SIMPLE=0, ALGORITHM_INSGPS_INDOOR_NOMAG=1, ALGORITHM_INSGPS_INDOOR=2, ALGORITHM_INSGPS_OUTDOOR=3 } AlgorithmOptions;
// Field Downsampling information
// Field UpdatePeriod information
// Field BiasCorrectedRaw information
/* Enumeration options for field BiasCorrectedRaw */
typedef enum { BIASCORRECTEDRAW_TRUE=0, BIASCORRECTEDRAW_FALSE=1 } BiasCorrectedRawOptions;
// Field YawBias information
// Field PitchBias information
// Field RollBias information
// Constants
static const quint32 OBJID = 1456050280U;
static const quint32 OBJID = 3741078856U;
static const QString NAME;
static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 1;

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

@ -71,6 +71,18 @@ _fields = [ \
{
}
),
uavobject.UAVObjectField(
'BiasCorrectedRaw',
'b',
1,
[
'0',
],
{
'0' : 'TRUE',
'1' : 'FALSE',
}
),
uavobject.UAVObjectField(
'YawBias',
'f',
@ -106,7 +118,7 @@ _fields = [ \
class AHRSSettings(uavobject.UAVObject):
## Object constants
OBJID = 1456050280
OBJID = 3741078856
NAME = "AHRSSettings"
METANAME = "AHRSSettingsMeta"
ISSINGLEINST = 1

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

@ -4,6 +4,7 @@
<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="UpdatePeriod" units="ms" type="uint8" elements="1" defaultvalue="1"/>
<field name="BiasCorrectedRaw" units="" type="enum" elements="1" options="TRUE,FALSE" defaultvalue="TRUE"/>
<field name="YawBias" units="" type="float" elements="1" defaultvalue="0"/>
<field name="PitchBias" units="" type="float" elements="1" defaultvalue="0"/>
<field name="RollBias" units="" type="float" elements="1" defaultvalue="0"/>