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

Merged in f5soh/librepilot/LP-613_cfGPS_algo (pull request #533)

Browse Source 
LP-613 cfGPS algo

Approved-by: Lalanne Laurent <f5soh@free.fr>
Approved-by: Vladimir Zidar <mr_w@mindnever.org>
This commit is contained in:
Lalanne Laurent 2019-04-30 18:19:48 +00:00
commit f8d23ad645
6 changed files with 91 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
flight/libraries/sanitycheck.c
View File

@ -77,6 +77,9 @@ int32_t configuration_check()
RevoSettingsFusionAlgorithmGet(&revoFusion);
bool navCapableFusion;
switch (revoFusion) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYGPSOUTDOOR:
navCapableFusion = (GetCurrentFrameType() == FRAME_TYPE_FIXED_WING) ? true : false;
break;
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13:
case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13CF:

68
flight/modules/StateEstimation/filtercf.c
View File

@ -64,13 +64,16 @@ struct data {
HomeLocationData homeLocation;
bool first_run;
bool useMag;
bool useVelHeading;
float currentAccel[3];
float currentMag[3];
float currentVel[3];
float accels_filtered[3];
float grot_filtered[3];
float gyroBias[3];
bool accelUpdated;
bool magUpdated;
bool velUpdated;
float accel_alpha;
bool accel_filter_enabled;
float rollPitchBiasRate;
@ -90,9 +93,10 @@ static FlightStatusData flightStatus;
static int32_t initwithmag(stateFilter *self);
static int32_t initwithoutmag(stateFilter *self);
static int32_t initwithvelheading(stateFilter *self);
static int32_t maininit(stateFilter *self);
static filterResult filter(stateFilter *self, stateEstimation *state);
static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4]);
static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float velheading[2], float attitude[4]);
static void flightStatusUpdatedCb(UAVObjEvent *ev);
@ -118,6 +122,15 @@ int32_t filterCFInitialize(stateFilter *handle)
return STACK_REQUIRED;
}
int32_t filterCFHInitialize(stateFilter *handle)
{
globalInit();
handle->init = &initwithvelheading;
handle->filter = &filter;
handle->localdata = pios_malloc(sizeof(struct data));
return STACK_REQUIRED;
}
int32_t filterCFMInitialize(stateFilter *handle)
{
globalInit();
@ -132,6 +145,16 @@ static int32_t initwithmag(stateFilter *self)
struct data *this = (struct data *)self->localdata;
this->useMag = 1;
this->useVelHeading = 0;
return maininit(self);
}
static int32_t initwithvelheading(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->useMag = 0;
this->useVelHeading = 1;
return maininit(self);
}
@ -140,6 +163,7 @@ static int32_t initwithoutmag(stateFilter *self)
struct data *this = (struct data *)self->localdata;
this->useMag = 0;
this->useVelHeading = 0;
return maininit(self);
}
@ -184,6 +208,12 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
this->currentMag[1] = state->mag[1];
this->currentMag[2] = state->mag[2];
}
if (IS_SET(state->updated, SENSORUPDATES_vel)) {
this->velUpdated = 1;
this->currentVel[0] = state->vel[0];
this->currentVel[1] = state->vel[1];
this->currentVel[2] = state->vel[2];
}
if (IS_SET(state->updated, SENSORUPDATES_accel)) {
this->accelUpdated = 1;
this->currentAccel[0] = state->accel[0];
@ -193,7 +223,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
if (IS_SET(state->updated, SENSORUPDATES_gyro)) {
if (this->accelUpdated) {
float attitude[4];
result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, attitude);
result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, this->currentVel, attitude);
if (result == FILTERRESULT_OK) {
state->attitude[0] = attitude[0];
state->attitude[1] = attitude[1];
@ -203,6 +233,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
}
this->accelUpdated = 0;
this->magUpdated = 0;
this->velUpdated = 0;
}
}
return result;
@ -222,7 +253,7 @@ static inline void apply_accel_filter(const struct data *this, const float *raw,
}
}
static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4])
static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float vel[3], float attitude[4])
{
float dT;
@ -396,7 +427,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
accel_err[2] /= (accel_mag * grot_mag);
float mag_err[3] = { 0.0f };
if (this->magUpdated && this->useMag) {
if (this->magUpdated && this->useMag && !this->useVelHeading) {
// Rotate gravity to body frame and cross with accels
float brot[3];
float Rbe[3][3];
@ -421,6 +452,31 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
} else {
CrossProduct((const float *)mag, (const float *)brot, mag_err);
}
} else if (this->velUpdated && this->useVelHeading) {
float speed = sqrtf(vel[0] * vel[0] + vel[1] * vel[1]);
if (speed > 3.0f) { // ~11kmh
float rpy[3];
Quaternion2RPY(attitude, rpy);
float heading = RAD2DEG(atan2f(-vel[1], -vel[0])) + 180.0f;
// find shortest way
float modulo = fmodf((heading - rpy[2]) + 180.0f, 360.0f);
if (modulo < 0) {
modulo += 180.0f;
} else {
modulo -= 180.0f;
}
/* From dRonin: The 0.008 is chosen to put things in a somewhat similar scale
* to the cross product. A 45 degree heading error will become
* a 7 deg/sec correction, so it neither takes forever to
* correct nor does a second of bad heading data screw us up
* too badly.
*/
mag_err[2] = modulo * 0.008f;
} else {
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
}
} else {
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
}
@ -433,7 +489,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
this->gyroBias[0] -= accel_err[0] * this->attitudeSettings.AccelKi - gyro[0] * this->rollPitchBiasRate;
this->gyroBias[1] -= accel_err[1] * this->attitudeSettings.AccelKi - gyro[1] * this->rollPitchBiasRate;
if (this->useMag) {
if (this->useMag || this->useVelHeading) {
this->gyroBias[2] -= mag_err[2] * this->attitudeSettings.MagKi - gyro[2] * this->rollPitchBiasRate;
} else {
this->gyroBias[2] -= -gyro[2] * this->rollPitchBiasRate;
@ -443,7 +499,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
// Correct rates based on proportional coefficient
gyrotmp[0] += accel_err[0] * this->attitudeSettings.AccelKp / dT;
gyrotmp[1] += accel_err[1] * this->attitudeSettings.AccelKp / dT;
if (this->useMag) {
if (this->useMag || this->useVelHeading) {
gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT + mag_err[2] * this->attitudeSettings.MagKp / dT;
} else {
gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT;

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

@ -95,6 +95,7 @@ int32_t filterAirInitialize(stateFilter *handle);
int32_t filterStationaryInitialize(stateFilter *handle);
int32_t filterLLAInitialize(stateFilter *handle);
int32_t filterCFInitialize(stateFilter *handle);
int32_t filterCFHInitialize(stateFilter *handle);
int32_t filterCFMInitialize(stateFilter *handle);
int32_t filterEKF13iInitialize(stateFilter *handle);
int32_t filterEKF13Initialize(stateFilter *handle);

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

@ -154,6 +154,7 @@ static stateFilter airFilter;
static stateFilter stationaryFilter;
static stateFilter llaFilter;
static stateFilter cfFilter;
static stateFilter cfhFilter;
static stateFilter cfmFilter;
static stateFilter ekf13iFilter;
static stateFilter ekf13Filter;
@ -184,6 +185,22 @@ static const filterPipeline *cfQueue = &(filterPipeline) {
}
}
};
static const filterPipeline *cfgpsQueue = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &llaFilter,
.next = &(filterPipeline) {
.filter = &baroiFilter,
.next = &(filterPipeline) {
.filter = &altitudeFilter,
.next = &(filterPipeline) {
.filter = &cfhFilter,
.next = NULL,
}
}
}
}
};
static const filterPipeline *cfmiQueue = &(filterPipeline) {
.filter = &magFilter,
.next = &(filterPipeline) {
@ -366,6 +383,7 @@ int32_t StateEstimationInitialize(void)
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, filterCFHInitialize(&cfhFilter));
stack_required = maxint32_t(stack_required, filterCFMInitialize(&cfmFilter));
stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter));
stack_required = maxint32_t(stack_required, filterEKF13Initialize(&ekf13Filter));
@ -443,6 +461,11 @@ static void StateEstimationCb(void)
// reinit Mag alarm
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_UNINITIALISED);
break;
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYGPSOUTDOOR:
newFilterChain = cfgpsQueue;
// reinit Mag alarm
AlarmsSet(SYSTEMALARMS_ALARM_MAGNETOMETER, SYSTEMALARMS_ALARM_UNINITIALISED);
break;
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG:
newFilterChain = cfmiQueue;
break;

2
ground/gcs/src/share/qml/js/uav.js
View File

@ -287,7 +287,7 @@ function gpsStatus() {
}
function fusionAlgorithm() {
var fusionAlgorithmText = ["None", "Basic (No Nav)", "CompMag", "Comp+Mag+GPS", "EKFIndoor", "GPSNav (INS)", "GPSNav (INS+CF)", "Testing (INS Indoor+CF)"];
var fusionAlgorithmText = ["None", "Basic (No Nav)", "Comp+GPS", "CompMag", "Comp+Mag+GPS", "EKFIndoor", "GPSNav (INS)", "GPSNav (INS+CF)", "Testing (INS Indoor+CF)", "Acro"];
if (fusionAlgorithmText.length != RevoSettings.RevoSettingsConstants.FusionAlgorithmCount) {
console.log("uav.js: fusionAlgorithm() do not match revoSettings.fusionAlgorithm uavo");

2
shared/uavobjectdefinition/revosettings.xml
View File

@ -2,7 +2,7 @@
<object name="RevoSettings" singleinstance="true" settings="true" category="State">
<description>Settings for the revo to control the algorithm and what is updated</description>
<field name="FusionAlgorithm" units="" type="enum" elements="1"
options="None,Basic (Complementary),Complementary+Mag,Complementary+Mag+GPSOutdoor,INS13Indoor,GPS Navigation (INS13),GPS Navigation (INS13+CF),Testing (INS Indoor+CF),Acro (No Sensors)"
options="None,Basic (Complementary),Complementary+GPSOutdoor,Complementary+Mag,Complementary+Mag+GPSOutdoor,INS13Indoor,GPS Navigation (INS13),GPS Navigation (INS13+CF),Testing (INS Indoor+CF),Acro (No Sensors)"
defaultvalue="Basic (Complementary)"/>
<!-- Low pass filter configuration to calculate offset of barometric altitude sensor.