Merge remote-tracking branch 'origin/develop' into acs_update_scheduling
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
This commit is contained in:
commit
4412690652
|
|
@ -17,6 +17,10 @@ change warranting a new major release:
|
|||
|
||||
# [unreleased]
|
||||
|
||||
## Changed
|
||||
|
||||
- Readded calibration matrices for MGM calibration
|
||||
|
||||
## Fixed
|
||||
|
||||
- Single sourcing the version information into `CMakeLists.txt`. The `git describe` functionality
|
||||
|
|
@ -25,6 +29,9 @@ change warranting a new major release:
|
|||
stored as the git SHA hash in `commonConfig.h`. This will be
|
||||
an empty string now for regular versions.
|
||||
- Bump FSFW for important fix in PUS mode service.
|
||||
- Bugfixes in 'SensofProcessing' where previously MGM values would be calibrated before being
|
||||
transformed in body RF. However, the calibration values are in the body RF. Also fixed the
|
||||
validity flag of 'mgmVecTotDerivative'.
|
||||
|
||||
# [v1.25.0] 2023-02-06
|
||||
|
||||
|
|
|
|||
|
|
@ -34,7 +34,6 @@ ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_
|
|||
uint32_t *msToReachTheMode) {
|
||||
if (not modeCommanded) {
|
||||
if (mode == MODE_ON or mode == MODE_OFF) {
|
||||
gpsNotOpenSwitch = true;
|
||||
// 5h time to reach fix
|
||||
*msToReachTheMode = MAX_SECONDS_TO_REACH_FIX;
|
||||
maxTimeToReachFix.resetTimer();
|
||||
|
|
@ -44,7 +43,12 @@ ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_
|
|||
}
|
||||
}
|
||||
if (mode == MODE_OFF) {
|
||||
PoolReadGuard pg(&gpsSet);
|
||||
gpsSet.setValidity(false, true);
|
||||
// There can't be a fix with a device that is off.
|
||||
triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, 0);
|
||||
oneShotSwitches.reset();
|
||||
modeCommanded = false;
|
||||
}
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
|
@ -101,6 +105,7 @@ ReturnValue_t GpsHyperionLinuxController::performOperation(uint8_t opCode) {
|
|||
if (not callAgainImmediately) {
|
||||
handleQueue();
|
||||
poolManager.performHkOperation();
|
||||
TaskFactory::delayTask(250);
|
||||
}
|
||||
}
|
||||
// Should never be reached.
|
||||
|
|
@ -113,25 +118,24 @@ ReturnValue_t GpsHyperionLinuxController::initialize() {
|
|||
return result;
|
||||
}
|
||||
auto openError = [&](const char *type, int error) {
|
||||
if (gpsNotOpenSwitch) {
|
||||
// Opening failed
|
||||
// Opening failed
|
||||
#if FSFW_VERBOSE_LEVEL >= 1
|
||||
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM " << type
|
||||
<< " failed | Error " << error << " | " << gps_errstr(error) << std::endl;
|
||||
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM " << type
|
||||
<< " failed | Error " << error << " | " << gps_errstr(error) << std::endl;
|
||||
#endif
|
||||
gpsNotOpenSwitch = false;
|
||||
}
|
||||
};
|
||||
if (readMode == ReadModes::SOCKET) {
|
||||
int retval = gps_open("localhost", DEFAULT_GPSD_PORT, &gps);
|
||||
if (retval != 0) {
|
||||
openError("Socket", retval);
|
||||
return ObjectManager::CHILD_INIT_FAILED;
|
||||
}
|
||||
gps_stream(&gps, WATCH_ENABLE | WATCH_JSON, nullptr);
|
||||
} else if (readMode == ReadModes::SHM) {
|
||||
int retval = gps_open(GPSD_SHARED_MEMORY, "", &gps);
|
||||
if (retval != 0) {
|
||||
openError("SHM", retval);
|
||||
return ObjectManager::CHILD_INIT_FAILED;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
|
|
@ -145,32 +149,33 @@ void GpsHyperionLinuxController::performControlOperation() {}
|
|||
|
||||
bool GpsHyperionLinuxController::readGpsDataFromGpsd() {
|
||||
auto readError = [&]() {
|
||||
if (gpsReadFailedSwitch) {
|
||||
gpsReadFailedSwitch = false;
|
||||
if (oneShotSwitches.gpsReadFailedSwitch) {
|
||||
oneShotSwitches.gpsReadFailedSwitch = false;
|
||||
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed | "
|
||||
"Error "
|
||||
<< errno << " | " << gps_errstr(errno) << std::endl;
|
||||
}
|
||||
};
|
||||
// GPS is off, no point in reading data from GPSD.
|
||||
if(mode == MODE_OFF) {
|
||||
return false;
|
||||
}
|
||||
if (readMode == ReadModes::SOCKET) {
|
||||
// Perform other necessary handling if not data seen for 0.2 seconds.
|
||||
if (gps_waiting(&gps, 200000)) {
|
||||
// Poll the GPS.
|
||||
if (gps_waiting(&gps, 0)) {
|
||||
if (-1 == gps_read(&gps)) {
|
||||
readError();
|
||||
return false;
|
||||
}
|
||||
oneShotSwitches.gpsReadFailedSwitch = true;
|
||||
if (MODE_SET != (MODE_SET & gps.set)) {
|
||||
if (mode == MODE_ON) {
|
||||
if (noModeSetCntr >= 0) {
|
||||
noModeSetCntr++;
|
||||
}
|
||||
if (noModeSetCntr == 10) {
|
||||
// TODO: Trigger event here
|
||||
sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be "
|
||||
"read for 10 consecutive reads"
|
||||
<< std::endl;
|
||||
noModeSetCntr = -1;
|
||||
}
|
||||
if (mode != MODE_OFF and maxTimeToReachFix.hasTimedOut() and
|
||||
oneShotSwitches.cantGetFixSwitch) {
|
||||
sif::warning
|
||||
<< "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be set in allowed "
|
||||
<< maxTimeToReachFix.timeout / 1000 << " seconds" << std::endl;
|
||||
triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.timeout);
|
||||
oneShotSwitches.cantGetFixSwitch = false;
|
||||
// did not event get mode, nothing to see.
|
||||
return false;
|
||||
}
|
||||
|
|
@ -198,51 +203,58 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
|
|||
}
|
||||
|
||||
bool validFix = false;
|
||||
static_cast<void>(validFix);
|
||||
// 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
|
||||
int newFixMode = gps.fix.mode;
|
||||
if (newFixMode == 2 or newFixMode == 3) {
|
||||
if (gps.fix.mode == 2 or gps.fix.mode == 3) {
|
||||
validFix = true;
|
||||
}
|
||||
if (gpsSet.fixMode.value != newFixMode) {
|
||||
triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, newFixMode);
|
||||
if (gpsSet.fixMode.value != gps.fix.mode) {
|
||||
triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, gps.fix.mode);
|
||||
}
|
||||
gpsSet.fixMode.value = newFixMode;
|
||||
gpsSet.fixMode.value = gps.fix.mode;
|
||||
if (gps.fix.mode == 0 or gps.fix.mode == 1) {
|
||||
if (modeCommanded and maxTimeToReachFix.hasTimedOut()) {
|
||||
// We are supposed to be on and functioning, but not fix was found
|
||||
// We are supposed to be on and functioning, but no fix was found
|
||||
if (mode == MODE_ON or mode == MODE_NORMAL) {
|
||||
mode = MODE_OFF;
|
||||
}
|
||||
modeCommanded = false;
|
||||
}
|
||||
gpsSet.setValidity(false, true);
|
||||
} else if (gps.satellites_used > 0) {
|
||||
} else if (gps.satellites_used > 0 && validFix && mode != MODE_OFF) {
|
||||
gpsSet.setValidity(true, true);
|
||||
}
|
||||
|
||||
gpsSet.satInUse.value = gps.satellites_used;
|
||||
gpsSet.satInView.value = gps.satellites_visible;
|
||||
|
||||
bool latValid = false;
|
||||
if (std::isfinite(gps.fix.latitude)) {
|
||||
// Negative latitude -> South direction
|
||||
gpsSet.latitude.value = gps.fix.latitude;
|
||||
} else {
|
||||
gpsSet.latitude.setValid(false);
|
||||
if (gps.fix.mode >= 2) {
|
||||
latValid = true;
|
||||
}
|
||||
}
|
||||
gpsSet.latitude.setValid(latValid);
|
||||
|
||||
bool longValid = false;
|
||||
if (std::isfinite(gps.fix.longitude)) {
|
||||
// Negative longitude -> West direction
|
||||
gpsSet.longitude.value = gps.fix.longitude;
|
||||
} else {
|
||||
gpsSet.longitude.setValid(false);
|
||||
if (gps.fix.mode >= 2) {
|
||||
longValid = true;
|
||||
}
|
||||
}
|
||||
gpsSet.latitude.setValid(longValid);
|
||||
|
||||
bool altitudeValid = false;
|
||||
if (std::isfinite(gps.fix.altitude)) {
|
||||
gpsSet.altitude.value = gps.fix.altitude;
|
||||
} else {
|
||||
gpsSet.altitude.setValid(false);
|
||||
if (gps.fix.mode == 3) {
|
||||
altitudeValid = true;
|
||||
}
|
||||
}
|
||||
gpsSet.altitude.setValid(altitudeValid);
|
||||
|
||||
if (std::isfinite(gps.fix.speed)) {
|
||||
gpsSet.speed.value = gps.fix.speed;
|
||||
|
|
@ -250,59 +262,44 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
|
|||
gpsSet.speed.setValid(false);
|
||||
}
|
||||
|
||||
if (TIME_SET == (TIME_SET & gps.set)) {
|
||||
timeval time = {};
|
||||
#if LIBGPS_VERSION_MINOR <= 17
|
||||
gpsSet.unixSeconds.value = gps.fix.time;
|
||||
gpsSet.unixSeconds.value = std::floor(gps.fix.time);
|
||||
double fractionalPart = gps.fix.time - gpsSet.unixSeconds.value;
|
||||
time.tv_usec = fractionalPart * 1000.0 * 1000.0;
|
||||
#else
|
||||
gpsSet.unixSeconds.value = gps.fix.time.tv_sec;
|
||||
gpsSet.unixSeconds.value = gps.fix.time.tv_sec;
|
||||
time.tv_usec = gps.fix.time.tv_nsec / 1000;
|
||||
#endif
|
||||
timeval time = {};
|
||||
time.tv_sec = gpsSet.unixSeconds.value;
|
||||
#if LIBGPS_VERSION_MINOR <= 17
|
||||
double fractionalPart = gps.fix.time - std::floor(gps.fix.time);
|
||||
time.tv_usec = fractionalPart * 1000.0 * 1000.0;
|
||||
#else
|
||||
time.tv_usec = gps.fix.time.tv_nsec / 1000;
|
||||
#endif
|
||||
std::time_t t = std::time(nullptr);
|
||||
if (time.tv_sec == t) {
|
||||
timeIsConstantCounter++;
|
||||
time.tv_sec = gpsSet.unixSeconds.value;
|
||||
// If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC
|
||||
// and no time file available) we set it with the roughly valid time from the GPS.
|
||||
// NTP might only work if the time difference between sys time and current time is not too
|
||||
// large.
|
||||
overwriteTimeIfNotSane(time, validFix);
|
||||
Clock::TimeOfDay_t timeOfDay = {};
|
||||
Clock::convertTimevalToTimeOfDay(&time, &timeOfDay);
|
||||
gpsSet.year = timeOfDay.year;
|
||||
gpsSet.month = timeOfDay.month;
|
||||
gpsSet.day = timeOfDay.day;
|
||||
gpsSet.hours = timeOfDay.hour;
|
||||
gpsSet.minutes = timeOfDay.minute;
|
||||
gpsSet.seconds = timeOfDay.second;
|
||||
} else {
|
||||
timeIsConstantCounter = 0;
|
||||
}
|
||||
if (timeInit and validFix) {
|
||||
if (not utility::timeSanityCheck()) {
|
||||
#if OBSW_VERBOSE_LEVEL >= 1
|
||||
time_t timeRaw = time.tv_sec;
|
||||
std::tm *timeTm = std::gmtime(&timeRaw);
|
||||
sif::info << "Setting invalid system time from GPS data directly: "
|
||||
<< std::put_time(timeTm, "%c %Z") << std::endl;
|
||||
#endif
|
||||
// For some reason, the clock needs to be somewhat correct for NTP to work. Really dumb..
|
||||
Clock::setClock(&time);
|
||||
}
|
||||
timeInit = false;
|
||||
}
|
||||
// If the received time does not change anymore for whatever reason, do not set it here
|
||||
// to avoid stale times. Also, don't do it too often often to avoid jumping times
|
||||
if (timeIsConstantCounter < 20 and timeUpdateCd.hasTimedOut()) {
|
||||
// Update the system time here for now. NTP seems to be unable to do so for whatever reason.
|
||||
// Further tests have shown that the time seems to be set by NTPD after some time..
|
||||
// Clock::setClock(&time);
|
||||
timeUpdateCd.resetTimer();
|
||||
gpsSet.unixSeconds.setValid(false);
|
||||
gpsSet.year.setValid(false);
|
||||
gpsSet.month.setValid(false);
|
||||
gpsSet.day.setValid(false);
|
||||
gpsSet.hours.setValid(false);
|
||||
gpsSet.minutes.setValid(false);
|
||||
gpsSet.seconds.setValid(false);
|
||||
}
|
||||
|
||||
Clock::TimeOfDay_t timeOfDay = {};
|
||||
Clock::convertTimevalToTimeOfDay(&time, &timeOfDay);
|
||||
gpsSet.year = timeOfDay.year;
|
||||
gpsSet.month = timeOfDay.month;
|
||||
gpsSet.day = timeOfDay.day;
|
||||
gpsSet.hours = timeOfDay.hour;
|
||||
gpsSet.minutes = timeOfDay.minute;
|
||||
gpsSet.seconds = timeOfDay.second;
|
||||
if (debugHyperionGps) {
|
||||
sif::info << "-- Hyperion GPS Data --" << std::endl;
|
||||
#if LIBGPS_VERSION_MINOR <= 17
|
||||
time_t timeRaw = gps.fix.time;
|
||||
time_t timeRaw = gpsSet.unixSeconds.value;
|
||||
#else
|
||||
time_t timeRaw = gps.fix.time.tv_sec;
|
||||
#endif
|
||||
|
|
@ -325,3 +322,19 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
|
|||
}
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
void GpsHyperionLinuxController::overwriteTimeIfNotSane(timeval time, bool validFix) {
|
||||
if (not timeInit and validFix) {
|
||||
if (not utility::timeSanityCheck()) {
|
||||
#if OBSW_VERBOSE_LEVEL >= 1
|
||||
time_t timeRaw = time.tv_sec;
|
||||
std::tm *timeTm = std::gmtime(&timeRaw);
|
||||
sif::info << "Overwriting invalid system time from GPS data directly: "
|
||||
<< std::put_time(timeTm, "%c %Z") << std::endl;
|
||||
#endif
|
||||
// For some reason, the clock needs to be somewhat correct for NTP to work. Really dumb..
|
||||
Clock::setClock(&time);
|
||||
}
|
||||
timeInit = true;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -58,18 +58,30 @@ class GpsHyperionLinuxController : public ExtendedControllerBase {
|
|||
const char* currentClientBuf = nullptr;
|
||||
ReadModes readMode = ReadModes::SOCKET;
|
||||
Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000);
|
||||
bool modeCommanded = true;
|
||||
bool timeInit = true;
|
||||
bool gpsNotOpenSwitch = true;
|
||||
bool gpsReadFailedSwitch = true;
|
||||
bool modeCommanded = false;
|
||||
bool timeInit = false;
|
||||
|
||||
struct OneShotSwitches {
|
||||
void reset() {
|
||||
gpsReadFailedSwitch = true;
|
||||
cantGetFixSwitch = true;
|
||||
}
|
||||
bool gpsReadFailedSwitch = true;
|
||||
bool cantGetFixSwitch = true;
|
||||
|
||||
} oneShotSwitches;
|
||||
|
||||
bool debugHyperionGps = false;
|
||||
int32_t noModeSetCntr = 0;
|
||||
uint32_t timeIsConstantCounter = 0;
|
||||
Countdown timeUpdateCd = Countdown(60);
|
||||
|
||||
// Returns true if the function should be called again or false if other
|
||||
// controller handling can be done.
|
||||
bool readGpsDataFromGpsd();
|
||||
// If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC)
|
||||
// we set it with the roughly valid time from the GPS. For some reason, NTP might only work
|
||||
// if the time difference between sys time and current time is not too large
|
||||
void overwriteTimeIfNotSane(timeval time, bool validFix);
|
||||
};
|
||||
|
||||
#endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */
|
||||
|
|
|
|||
|
|
@ -426,7 +426,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
|
|||
return INVALID_SUBMODE;
|
||||
}
|
||||
} else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) {
|
||||
if (submode < acs::AcsModes::SAFE or submode > acs::AcsModes::PTG_TARGET_INERTIAL) {
|
||||
if ((submode < acs::AcsModes::SAFE) or (submode > acs::AcsModes::PTG_TARGET_INERTIAL)) {
|
||||
return INVALID_SUBMODE;
|
||||
} else {
|
||||
return returnvalue::OK;
|
||||
|
|
|
|||
|
|
@ -37,26 +37,28 @@ class AcsParameters /*: public HasParametersIF*/ {
|
|||
float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}};
|
||||
float mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
|
||||
|
||||
float mgm0hardIronOffset[3] = {0.0, 0.0, 0.0}; //{19.89364, -29.94111, -31.07508};
|
||||
float mgm1hardIronOffset[3] = {0.0, 0.0, 0.0}; //{10.95500, -8.053403, -33.36383};
|
||||
float mgm2hardIronOffset[3] = {0.0, 0.0, 0.0}; //{15.72181, -26.87090, -62.19010};
|
||||
float mgm3hardIronOffset[3] = {0.0, 0.0, 0.0};
|
||||
float mgm4hardIronOffset[3] = {0.0, 0.0, 0.0};
|
||||
float mgm0hardIronOffset[3] = {6.116487, 6.796264, -19.188060};
|
||||
float mgm1hardIronOffset[3] = {-1.077152, 2.080583, 1.974483};
|
||||
float mgm2hardIronOffset[3] = {-19.285857, 5.401821, -16.096297};
|
||||
float mgm3hardIronOffset[3] = {-0.634033, 2.787695, 0.092036};
|
||||
float mgm4hardIronOffset[3] = {2.702743, 5.236043, 0.726229};
|
||||
|
||||
float mgm0softIronInverse[3][3] = {{0.910192, -0.188413, -0.161522},
|
||||
{-0.188413, 1.642303, -0.033184},
|
||||
{-0.161522, -0.033184, 0.943904}};
|
||||
float mgm1softIronInverse[3][3] = {{1.053508, -0.170225, -0.041678},
|
||||
{-0.170225, 1.274465, -0.040231},
|
||||
{-0.041678, -0.040231, 1.086352}};
|
||||
float mgm2softIronInverse[3][3] = {{0.931086, 0.172675, -0.043084},
|
||||
{0.172675, 1.541296, 0.065489},
|
||||
{-0.043084, 0.065489, 1.001238}};
|
||||
float mgm3softIronInverse[3][3] = {{1.073353, 0.177266, -0.058832},
|
||||
{0.177266, 1.262156, 0.010478},
|
||||
{-0.058832, 0.010478, 1.068345}};
|
||||
float mgm4softIronInverse[3][3] = {{1.114887, -0.007534, -0.037072},
|
||||
{-0.007534, 1.253879, 0.006812},
|
||||
{-0.037072, 0.006812, 1.313158}};
|
||||
|
||||
float mgm0softIronInverse[3][3] = {
|
||||
{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; /*{{1420.727e-3, 9.352825e-3,
|
||||
-127.1979e-3}, {9.352825e-3, 1031.965e-3, -80.02734e-3},
|
||||
{-127.1979e-3, -80.02734e-3, 934.8899e-3}};*/
|
||||
float mgm1softIronInverse[3][3] = {
|
||||
{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; /*{{126.7325e-2, -4.146410e-2, -18.37963e-2},
|
||||
{-4.146410e-2, 109.3310e-2, -5.246314e-2},
|
||||
{-18.37963e-2, -5.246314e-2, 105.7300e-2}};*/
|
||||
float mgm2softIronInverse[3][3] = {
|
||||
{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; /*{{143.0438e-2, 7.095763e-2,
|
||||
15.67482e-2}, {7.095763e-2, 99.65167e-2, -6.958760e-2},
|
||||
{15.67482e-2, -6.958760e-2, 94.50124e-2}};*/
|
||||
float mgm3softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
|
||||
float mgm4softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
|
||||
float mgm02variance[3] = {1, 1, 1};
|
||||
float mgm13variance[3] = {1, 1, 1};
|
||||
float mgm4variance[3] = {1, 1, 1};
|
||||
|
|
|
|||
|
|
@ -47,71 +47,72 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
|||
float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0},
|
||||
mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0},
|
||||
mgm4ValueNoBias[3] = {0, 0, 0};
|
||||
float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0},
|
||||
mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
|
||||
float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0},
|
||||
mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0};
|
||||
float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0},
|
||||
mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
|
||||
float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
|
||||
|
||||
if (mgm0valid) {
|
||||
VectorOperations<float>::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias,
|
||||
3);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value,
|
||||
mgm0ValueBody, 3, 3, 1);
|
||||
VectorOperations<float>::subtract(mgm0ValueBody, mgmParameters->mgm0hardIronOffset,
|
||||
mgm0ValueNoBias, 3);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm0softIronInverse[0], mgm0ValueNoBias,
|
||||
mgm0ValueCalib, 3, 3, 1);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib,
|
||||
mgm0ValueBody, 3, 3, 1);
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
sensorFusionNumerator[i] += mgm0ValueBody[i] / mgmParameters->mgm02variance[i];
|
||||
sensorFusionNumerator[i] += mgm0ValueCalib[i] / mgmParameters->mgm02variance[i];
|
||||
sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
|
||||
}
|
||||
}
|
||||
if (mgm1valid) {
|
||||
VectorOperations<float>::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias,
|
||||
3);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value,
|
||||
mgm1ValueBody, 3, 3, 1);
|
||||
VectorOperations<float>::subtract(mgm1ValueBody, mgmParameters->mgm1hardIronOffset,
|
||||
mgm1ValueNoBias, 3);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm1softIronInverse[0], mgm1ValueNoBias,
|
||||
mgm1ValueCalib, 3, 3, 1);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib,
|
||||
mgm1ValueBody, 3, 3, 1);
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
sensorFusionNumerator[i] += mgm1ValueBody[i] / mgmParameters->mgm13variance[i];
|
||||
sensorFusionNumerator[i] += mgm1ValueCalib[i] / mgmParameters->mgm13variance[i];
|
||||
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
|
||||
}
|
||||
}
|
||||
if (mgm2valid) {
|
||||
VectorOperations<float>::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias,
|
||||
3);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value,
|
||||
mgm2ValueBody, 3, 3, 1);
|
||||
VectorOperations<float>::subtract(mgm2ValueBody, mgmParameters->mgm2hardIronOffset,
|
||||
mgm2ValueNoBias, 3);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm2softIronInverse[0], mgm2ValueNoBias,
|
||||
mgm2ValueCalib, 3, 3, 1);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib,
|
||||
mgm2ValueBody, 3, 3, 1);
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
sensorFusionNumerator[i] += mgm2ValueBody[i] / mgmParameters->mgm02variance[i];
|
||||
sensorFusionNumerator[i] += mgm2ValueCalib[i] / mgmParameters->mgm02variance[i];
|
||||
sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
|
||||
}
|
||||
}
|
||||
if (mgm3valid) {
|
||||
VectorOperations<float>::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias,
|
||||
3);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value,
|
||||
mgm3ValueBody, 3, 3, 1);
|
||||
VectorOperations<float>::subtract(mgm3ValueBody, mgmParameters->mgm3hardIronOffset,
|
||||
mgm3ValueNoBias, 3);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm3softIronInverse[0], mgm3ValueNoBias,
|
||||
mgm3ValueCalib, 3, 3, 1);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib,
|
||||
mgm3ValueBody, 3, 3, 1);
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
sensorFusionNumerator[i] += mgm3ValueBody[i] / mgmParameters->mgm13variance[i];
|
||||
sensorFusionNumerator[i] += mgm3ValueCalib[i] / mgmParameters->mgm13variance[i];
|
||||
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
|
||||
}
|
||||
}
|
||||
if (mgm4valid) {
|
||||
float mgm4ValueNT[3];
|
||||
VectorOperations<float>::mulScalar(mgm4Value, 1e-3, mgm4ValueNT, 3); // uT to nT
|
||||
VectorOperations<float>::subtract(mgm4ValueNT, mgmParameters->mgm4hardIronOffset,
|
||||
float mgm4ValueUT[3];
|
||||
VectorOperations<float>::mulScalar(mgm4Value, 1e-3, mgm4ValueUT, 3); // nT to uT
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueUT,
|
||||
mgm4ValueBody, 3, 3, 1);
|
||||
VectorOperations<float>::subtract(mgm4ValueBody, mgmParameters->mgm4hardIronOffset,
|
||||
mgm4ValueNoBias, 3);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm4softIronInverse[0], mgm4ValueNoBias,
|
||||
mgm4ValueCalib, 3, 3, 1);
|
||||
MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib,
|
||||
mgm4ValueBody, 3, 3, 1);
|
||||
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
sensorFusionNumerator[i] += mgm4ValueBody[i] / mgmParameters->mgm4variance[i];
|
||||
sensorFusionNumerator[i] += mgm4ValueCalib[i] / mgmParameters->mgm4variance[i];
|
||||
sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i];
|
||||
}
|
||||
}
|
||||
|
|
@ -128,6 +129,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
|||
for (uint8_t i = 0; i < 3; i++) {
|
||||
mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff;
|
||||
savedMgmVecTot[i] = mgmVecTot[i];
|
||||
mgmVecTotDerivativeValid = true;
|
||||
}
|
||||
}
|
||||
timeOfSavedMagFieldEst = timeOfMgmMeasurement;
|
||||
|
|
@ -148,15 +150,15 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
|||
{
|
||||
PoolReadGuard pg(mgmDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueBody, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueCalib, 3 * sizeof(float));
|
||||
mgmDataProcessed->mgm0vec.setValid(mgm0valid);
|
||||
std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueBody, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueCalib, 3 * sizeof(float));
|
||||
mgmDataProcessed->mgm1vec.setValid(mgm1valid);
|
||||
std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueBody, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueCalib, 3 * sizeof(float));
|
||||
mgmDataProcessed->mgm2vec.setValid(mgm2valid);
|
||||
std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueBody, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueCalib, 3 * sizeof(float));
|
||||
mgmDataProcessed->mgm3vec.setValid(mgm3valid);
|
||||
std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueBody, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueCalib, 3 * sizeof(float));
|
||||
mgmDataProcessed->mgm4vec.setValid(mgm4valid);
|
||||
std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(double));
|
||||
mgmDataProcessed->mgmVecTot.setValid(true);
|
||||
|
|
|
|||
|
|
@ -13,6 +13,9 @@ static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::GPS_HANDLER;
|
|||
//! [EXPORT] : [COMMENT] Fix has changed. P1: Old fix. P2: New fix
|
||||
//! 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix
|
||||
static constexpr Event GPS_FIX_CHANGE = event::makeEvent(SUBSYSTEM_ID, 0, severity::INFO);
|
||||
//! [EXPORT] : [COMMENT] Could not get fix in maximum allowed time. P1: Maximum allowed time
|
||||
//! to get a fix after the GPS was switched on.
|
||||
static constexpr Event CANT_GET_FIX = event::makeEvent(SUBSYSTEM_ID, 0, severity::LOW);
|
||||
|
||||
static constexpr DeviceCommandId_t GPS_REPLY = 0;
|
||||
static constexpr DeviceCommandId_t TRIGGER_RESET_PIN_GNSS = 5;
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user