/**
* Module initialization
*/
int32_t StabilizationInitialize()
{
// Initialize variables
StabilizationDesiredInitialize();
StabilizationSettingsInitialize();
StabilizationStatusInitialize();
StabilizationBankInitialize();
StabilizationSettingsBank1Initialize();
StabilizationSettingsBank2Initialize();
StabilizationSettingsBank3Initialize();
RateDesiredInitialize();
ManualControlCommandInitialize(); // only used for PID bank selection based on flight mode switch
sin_lookup_initalize();
stabilizationOuterloopInit();
stabilizationInnerloopInit();
#ifdef REVOLUTION
stabilizationAltitudeloopInit();
#endif
pid_zero(&stabSettings.outerPids[0]);
pid_zero(&stabSettings.outerPids[1]);
pid_zero(&stabSettings.outerPids[2]);
pid_zero(&stabSettings.innerPids[0]);
pid_zero(&stabSettings.innerPids[1]);
pid_zero(&stabSettings.innerPids[2]);
return 0;
}
//! 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;
}
/**
* @brief initialize UAVOs and structs used by this library
*/
void plan_initialize()
{
TakeOffLocationInitialize();
PositionStateInitialize();
PathDesiredInitialize();
FlightModeSettingsInitialize();
AttitudeStateInitialize();
ManualControlCommandInitialize();
}
/**
* @brief initialize UAVOs and structs used by this library
*/
void plan_initialize()
{
TakeOffLocationInitialize();
PositionStateInitialize();
PathDesiredInitialize();
FlightModeSettingsInitialize();
FlightStatusInitialize();
AttitudeStateInitialize();
ManualControlCommandInitialize();
VelocityStateInitialize();
VtolPathFollowerSettingsInitialize();
}
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 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;
}
/**
* Function used to initialize the first instance of each object.
* This file is automatically updated by the UAVObjectGenerator.
*/
void UAVObjectsInitializeAll()
{
ActuatorCommandInitialize();
ActuatorDesiredInitialize();
ActuatorSettingsInitialize();
AHRSCalibrationInitialize();
AHRSSettingsInitialize();
AhrsStatusInitialize();
AttitudeActualInitialize();
AttitudeDesiredInitialize();
AttitudeRawInitialize();
AttitudeSettingsInitialize();
BaroAltitudeInitialize();
ExampleObject1Initialize();
ExampleObject2Initialize();
ExampleSettingsInitialize();
FlightBatteryStateInitialize();
FlightTelemetryStatsInitialize();
GCSTelemetryStatsInitialize();
GPSPositionInitialize();
GPSSatellitesInitialize();
GPSTimeInitialize();
HomeLocationInitialize();
ManualControlCommandInitialize();
ManualControlSettingsInitialize();
MixerSettingsInitialize();
MixerStatusInitialize();
ObjectPersistenceInitialize();
PositionActualInitialize();
StabilizationSettingsInitialize();
SystemAlarmsInitialize();
SystemSettingsInitialize();
SystemStatsInitialize();
TelemetrySettingsInitialize();
VTOLSettingsInitialize();
VTOLStatusInitialize();
}
