/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t FixedWingPathFollowerInitialize()
{
#ifdef MODULE_FixedWingPathFollower_BUILTIN
module_enabled = true;
#else
uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_state);
if (module_state[MODULESETTINGS_ADMINSTATE_FIXEDWINGPATHFOLLOWER] == MODULESETTINGS_ADMINSTATE_ENABLED) {
module_enabled = true;
} else {
module_enabled = false;
}
#endif
if (module_enabled) {
FixedWingPathFollowerSettingsInitialize();
FixedWingAirspeedsInitialize();
FixedWingPathFollowerStatusInitialize();
PathDesiredInitialize();
PathStatusInitialize();
VelocityDesiredInitialize();
AirspeedActualInitialize();
}
return 0;
}
/**
* Initialise the gps module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t GPSInitialize(void)
{
gpsPort = PIOS_COM_GPS;
uint8_t gpsProtocol;
#ifdef MODULE_GPS_BUILTIN
module_enabled = true;
#else
uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_state);
if (module_state[MODULESETTINGS_ADMINSTATE_GPS] == MODULESETTINGS_ADMINSTATE_ENABLED) {
module_enabled = true;
} else {
module_enabled = false;
}
#endif
// These things are only conditional on small F1 targets.
// Expected to be always present otherwise.
#ifdef SMALLF1
if (gpsPort && module_enabled) {
#endif
GPSPositionInitialize();
GPSVelocityInitialize();
#if !defined(PIOS_GPS_MINIMAL)
GPSTimeInitialize();
GPSSatellitesInitialize();
HomeLocationInitialize();
UBloxInfoInitialize();
#endif
#if defined(PIOS_GPS_PROVIDES_AIRSPEED)
AirspeedActualInitialize();
#endif
updateSettings();
#ifdef SMALLF1
}
#endif
if (gpsPort && module_enabled) {
ModuleSettingsGPSDataProtocolGet(&gpsProtocol);
switch (gpsProtocol) {
case MODULESETTINGS_GPSDATAPROTOCOL_NMEA:
gps_rx_buffer = PIOS_malloc(NMEA_MAX_PACKET_LENGTH);
break;
case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
gps_rx_buffer = PIOS_malloc(sizeof(struct UBXPacket));
break;
default:
gps_rx_buffer = NULL;
}
PIOS_Assert(gps_rx_buffer);
return 0;
}
return -1;
}
/**
* 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;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AirspeedInitialize()
{
#ifdef MODULE_Airspeed_BUILTIN
module_enabled = true;
#else
uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_state);
if (module_state[MODULESETTINGS_ADMINSTATE_AIRSPEED] == MODULESETTINGS_ADMINSTATE_ENABLED) {
module_enabled = true;
} else {
module_enabled = false;
}
#endif
if (!module_enabled)
return -1;
#ifdef BARO_AIRSPEED_PRESENT
ADCRoutingInitialize();
uint8_t adc_channel_map[ADCROUTING_CHANNELMAP_NUMELEM];
ADCRoutingChannelMapGet(adc_channel_map);
//Determine if the barometric airspeed sensor is routed to an ADC pin
for (int i = 0; i < ADCROUTING_CHANNELMAP_NUMELEM; i++) {
if (adc_channel_map[i] == ADCROUTING_CHANNELMAP_ANALOGAIRSPEED) {
airspeedADCPin = i;
}
}
#endif
BaroAirspeedInitialize();
AirspeedActualInitialize();
AirspeedSettingsInitialize();
AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb);
return 0;
}
/**
* Initialise the gps module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t GPSInitialize(void)
{
gpsPort = PIOS_COM_GPS;
uint8_t gpsProtocol;
#ifdef MODULE_GPS_BUILTIN
module_enabled = true;
#else
uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_state);
if (module_state[MODULESETTINGS_ADMINSTATE_GPS] == MODULESETTINGS_ADMINSTATE_ENABLED) {
module_enabled = true;
} else {
module_enabled = false;
}
#endif
#if defined(REVOLUTION)
// These objects MUST be initialized for Revolution
// because the rest of the system expects to just
// attach to their queues
GPSPositionInitialize();
GPSVelocityInitialize();
GPSTimeInitialize();
GPSSatellitesInitialize();
HomeLocationInitialize();
UBloxInfoInitialize();
updateSettings();
#else
if (gpsPort && module_enabled) {
GPSPositionInitialize();
GPSVelocityInitialize();
#if !defined(PIOS_GPS_MINIMAL)
GPSTimeInitialize();
GPSSatellitesInitialize();
#endif
#if defined(PIOS_GPS_PROVIDES_AIRSPEED)
AirspeedActualInitialize();
#endif
updateSettings();
}
#endif
if (gpsPort && module_enabled) {
ModuleSettingsGPSDataProtocolGet(&gpsProtocol);
switch (gpsProtocol) {
case MODULESETTINGS_GPSDATAPROTOCOL_NMEA:
gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH);
break;
case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
gps_rx_buffer = pvPortMalloc(sizeof(struct UBXPacket));
break;
default:
gps_rx_buffer = NULL;
}
PIOS_Assert(gps_rx_buffer);
return 0;
}
return -1;
}
