void AhrsInitHandles(void)
{
int idx = 0;
//Note: the first item in this list has the highest priority
//the last has the lowest priority
ADDHANDLE(idx++, AttitudeRaw);
ADDHANDLE(idx++, AttitudeActual);
ADDHANDLE(idx++, AHRSCalibration);
ADDHANDLE(idx++, AhrsStatus);
ADDHANDLE(idx++, BaroAltitude);
ADDHANDLE(idx++, GPSPosition);
ADDHANDLE(idx++, PositionActual);
ADDHANDLE(idx++, VelocityActual);
ADDHANDLE(idx++, HomeLocation);
ADDHANDLE(idx++, AHRSSettings);
if (idx != MAX_AHRS_OBJECTS) {
PIOS_DEBUG_Assert(0);
}
//Note: Only connect objects that the AHRS needs to read
//When the AHRS writes to these the data does a round trip
//AHRS->OP->AHRS due to these events
#ifndef IN_AHRS
AHRSSettingsConnectCallback(ObjectUpdatedCb);
BaroAltitudeConnectCallback(ObjectUpdatedCb);
GPSPositionConnectCallback(ObjectUpdatedCb);
HomeLocationConnectCallback(ObjectUpdatedCb);
AHRSCalibrationConnectCallback(ObjectUpdatedCb);
#endif
}
/**
* Initialise the module. Called before the start function
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AttitudeInitialize(void)
{
AttitudeActualInitialize();
AirspeedActualInitialize();
AirspeedSensorInitialize();
AttitudeSettingsInitialize();
PositionActualInitialize();
VelocityActualInitialize();
RevoSettingsInitialize();
RevoCalibrationInitialize();
EKFConfigurationInitialize();
EKFStateVarianceInitialize();
FlightStatusInitialize();
// Initialize this here while we aren't setting the homelocation in GPS
HomeLocationInitialize();
// Initialize quaternion
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
attitude.q1 = 1.0f;
attitude.q2 = 0.0f;
attitude.q3 = 0.0f;
attitude.q4 = 0.0f;
AttitudeActualSet(&attitude);
// Cannot trust the values to init right above if BL runs
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosBias.x = 0.0f;
gyrosBias.y = 0.0f;
gyrosBias.z = 0.0f;
GyrosBiasSet(&gyrosBias);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
RevoSettingsConnectCallback(&settingsUpdatedCb);
RevoCalibrationConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&settingsUpdatedCb);
EKFConfigurationConnectCallback(&settingsUpdatedCb);
FlightStatusConnectCallback(&settingsUpdatedCb);
return 0;
}
