//! Initialize the transmitter control mode
int32_t transmitter_control_initialize()
{
AccessoryDesiredInitialize();
ManualControlCommandInitialize();
FlightStatusInitialize();
StabilizationDesiredInitialize();
ReceiverActivityInitialize();
ManualControlSettingsInitialize();
// Both the gimbal and coptercontrol do not support loitering
#if !defined(SMALLF1) && !defined(GIMBAL)
LoiterCommandInitialize();
#endif
/* For now manual instantiate extra instances of Accessory Desired. In future */
/* should be done dynamically this includes not even registering it if not used */
AccessoryDesiredCreateInstance();
AccessoryDesiredCreateInstance();
/* No pending control events */
pending_control_event = CONTROL_EVENTS_NONE;
/* Initialize the RcvrActivty FSM */
lastActivityTime = PIOS_Thread_Systime();
resetRcvrActivity(&activity_fsm);
// Use callback to update the settings when they change
ManualControlSettingsConnectCallbackCtx(UAVObjCbSetFlag, &settings_updated);
settings_updated = true;
// Main task loop
lastSysTime = PIOS_Thread_Systime();
return 0;
}
/**
* Initialize the module, called on startup.
* \returns 0 on success or -1 if initialization failed
*/
int32_t SystemModInitialize(void)
{
// Must registers objects here for system thread because ObjectManager started in OpenPilotInit
SystemSettingsInitialize();
SystemStatsInitialize();
FlightStatusInitialize();
//ObjectPersistenceInitialize(); moved to pios board due to different bootstraping
#if defined(DIAG_TASKS)
TaskInfoInitialize();
#endif
#if defined(I2C_WDG_STATS_DIAGNOSTICS)
I2CStatsInitialize();
WatchdogStatusInitialize();
#endif
//objectPersistenceQueue = xQueueCreate(1, sizeof(UAVObjEvent));
//if (objectPersistenceQueue == NULL)
// return -1;
/* Edited for raspberry pilot, the systemodstart is not automatically called
* it is called just after MODULE_INITIALISE_ALL in initTask(), but the system
* waits for objectpersitance (over uavlink) to load all the settings
*/
//SystemModStart();
return 0;
}
/**
* @brief initialize UAVOs and structs used by this library
*/
void plan_initialize()
{
TakeOffLocationInitialize();
PositionStateInitialize();
PathDesiredInitialize();
FlightModeSettingsInitialize();
FlightStatusInitialize();
AttitudeStateInitialize();
ManualControlCommandInitialize();
VelocityStateInitialize();
VtolPathFollowerSettingsInitialize();
}
int32_t LoggingInitialize(void)
{
DebugLogSettingsInitialize();
DebugLogControlInitialize();
DebugLogStatusInitialize();
DebugLogEntryInitialize();
FlightStatusInitialize();
PIOS_DEBUGLOG_Initialize();
entry = pios_malloc(sizeof(DebugLogEntryData));
if (!entry) {
return -1;
}
return 0;
}
/**
* Initialize the module
* \return -1 if initialization failed
* \return 0 on success
*/
static int32_t uavoTaranisInitialize(void)
{
uint32_t sport_com = PIOS_COM_FRSKY_SPORT;
if (sport_com) {
frsky = PIOS_malloc(sizeof(struct frsky_sport_telemetry));
if (frsky != NULL) {
memset(frsky, 0x00, sizeof(struct frsky_sport_telemetry));
// These objects are registered on the TLM so it
// can intercept them from the telemetry stream
FlightBatteryStateInitialize();
FlightStatusInitialize();
PositionActualInitialize();
VelocityActualInitialize();
frsky->frsky_settings.use_current_sensor = false;
frsky->frsky_settings.batt_cell_count = 0;
frsky->frsky_settings.use_baro_sensor = false;
frsky->state = FRSKY_STATE_WAIT_POLL_REQUEST;
frsky->last_poll_time = PIOS_DELAY_GetuS();
frsky->ignore_rx_chars = 0;
frsky->scheduled_item = -1;
frsky->com = sport_com;
uint8_t i;
for (i = 0; i < NELEMENTS(frsky_value_items); i++)
frsky->item_last_triggered[i] = PIOS_DELAY_GetuS();
PIOS_COM_ChangeBaud(frsky->com, FRSKY_SPORT_BAUDRATE);
module_enabled = true;
return 0;
}
module_enabled = true;
return 0;
}
module_enabled = false;
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;
}
void stabilizationInnerloopInit()
{
RateDesiredInitialize();
ActuatorDesiredInitialize();
GyroStateInitialize();
StabilizationStatusInitialize();
FlightStatusInitialize();
ManualControlCommandInitialize();
#ifdef REVOLUTION
AirspeedStateInitialize();
AirspeedStateConnectCallback(AirSpeedUpdatedCb);
#endif
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationInnerloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION1, STACK_SIZE_BYTES);
GyroStateConnectCallback(GyroStateUpdatedCb);
// schedule dead calls every FAILSAFE_TIMEOUT_MS to have the watchdog cleared
PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
}
/**
* Initialize the module, called on startup.
* \returns 0 on success or -1 if initialization failed
*/
int32_t SystemModInitialize(void)
{
// Must registers objects here for system thread because ObjectManager started in OpenPilotInit
SystemSettingsInitialize();
SystemStatsInitialize();
FlightStatusInitialize();
ObjectPersistenceInitialize();
#if defined(DIAG_TASKS)
TaskInfoInitialize();
#endif
#if defined(DIAGNOSTICS)
I2CStatsInitialize();
WatchdogStatusInitialize();
#endif
SystemModStart();
return 0;
}
//! Initialize the transmitter control mode
int32_t transmitter_control_initialize()
{
/* Check the assumptions about uavobject enum's are correct */
if(!assumptions)
return -1;
AccessoryDesiredInitialize();
ManualControlCommandInitialize();
FlightStatusInitialize();
StabilizationDesiredInitialize();
ReceiverActivityInitialize();
ManualControlSettingsInitialize();
// Both the gimbal and coptercontrol do not support loitering
#if !defined(COPTERCONTROL) && !defined(GIMBAL)
LoiterCommandInitialize();
#endif
/* For now manual instantiate extra instances of Accessory Desired. In future */
/* should be done dynamically this includes not even registering it if not used */
AccessoryDesiredCreateInstance();
AccessoryDesiredCreateInstance();
/* Reset the state machine for processing stick movements */
arm_state = ARM_STATE_DISARMED;
/* No pending control events */
pending_control_event = CONTROL_EVENTS_NONE;
/* Initialize the RcvrActivty FSM */
lastActivityTime = xTaskGetTickCount();
resetRcvrActivity(&activity_fsm);
// Use callback to update the settings when they change
ManualControlSettingsConnectCallback(manual_control_settings_updated);
manual_control_settings_updated(NULL);
// Main task loop
lastSysTime = xTaskGetTickCount();
return 0;
}
/**
* Module initialization
*/
int32_t ManualControlInitialize()
{
/* Check the assumptions about uavobject enum's are correct */
PIOS_STATIC_ASSERT(assumptions);
ManualControlCommandInitialize();
FlightStatusInitialize();
ManualControlSettingsInitialize();
FlightModeSettingsInitialize();
SystemSettingsInitialize();
StabilizationSettingsInitialize();
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
VtolSelfTuningStatsInitialize();
VtolPathFollowerSettingsInitialize();
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
SystemSettingsConnectCallback(&SettingsUpdatedCb);
#endif
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES);
return 0;
}
/**
* Initialize the module, called on startup.
* \returns 0 on success or -1 if initialization failed
*/
int32_t SystemModInitialize(void)
{
// Must registers objects here for system thread because ObjectManager started in OpenPilotInit
SystemSettingsInitialize();
SystemStatsInitialize();
FlightStatusInitialize();
ObjectPersistenceInitialize();
#if defined(DIAG_TASKS)
TaskInfoInitialize();
#endif
#if defined(WDG_STATS_DIAGNOSTICS)
WatchdogStatusInitialize();
#endif
objectPersistenceQueue = PIOS_Queue_Create(1, sizeof(UAVObjEvent));
if (objectPersistenceQueue == NULL)
return -1;
SystemModStart();
return 0;
}
