/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AttitudeInitialize(void)
{
// Initialize quaternion
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
attitude.q1 = 1;
attitude.q2 = 0;
attitude.q3 = 0;
attitude.q4 = 0;
AttitudeActualSet(&attitude);
// Create queue for passing gyro data, allow 2 back samples in case
gyro_queue = xQueueCreate(1, sizeof(float) * 4);
if(gyro_queue == NULL)
return -1;
PIOS_ADC_SetQueue(gyro_queue);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
// Start main task
xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
return 0;
}
/**
* Start the task. Expects all objects to be initialized by this point.
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AttitudeStart(void)
{
// Create the queues for the sensors
gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
accelQueue = xQueueCreate(1, sizeof(UAVObjEvent));
magQueue = xQueueCreate(1, sizeof(UAVObjEvent));
airspeedQueue = xQueueCreate(1, sizeof(UAVObjEvent));
baroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent));
gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent));
// Start main task
xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &attitudeTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
#endif
GyrosConnectQueue(gyroQueue);
AccelsConnectQueue(accelQueue);
MagnetometerConnectQueue(magQueue);
AirspeedSensorConnectQueue(airspeedQueue);
BaroAltitudeConnectQueue(baroQueue);
GPSPositionConnectQueue(gpsQueue);
GPSVelocityConnectQueue(gpsVelQueue);
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AHRSCommsStart(void)
{
// Start main task
xTaskCreate(ahrscommsTask, (signed char *)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_AHRSCOMMS, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_AHRS);
return 0;
}
/**
* Start the task. Expects all objects to be initialized by this point.
* \returns 0 on success or -1 if initialisation failed
*/
int32_t SensorsStart(void)
{
// Start main task
xTaskCreate(SensorsTask, (signed char *)"Sensors", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &sensorsTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS);
return 0;
}
/**
* @brief Module initialization
* @return 0
*/
static int32_t BrushlessGimbalStart()
{
// Start main task
taskHandle = PIOS_Thread_Create(brushlessGimbalTask, "BrushlessGimbal", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_ACTUATOR, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR);
return 0;
}
/**
* Module starting
*/
int32_t ManualControlStart()
{
// Start main task
taskHandle = PIOS_Thread_Create(manualControlTask, "Control", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
return 0;
}
/**
* Module initialization
*/
int32_t StabilizationStart()
{
// Initialize variables
// Start main task
xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_STABILIZATION, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AttitudeStart(void)
{
// Start main task
xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AttitudeStart(void)
{
// Start main task
xTaskCreate(AttitudeTask, "Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, taskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
#endif
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AutotuneStart(void)
{
// Start main task if it is enabled
if(module_enabled) {
xTaskCreate(AutotuneTask, (signed char *)"Autotune", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_AUTOTUNE, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE);
}
return 0;
}
/**
* Start the task. Expects all objects to be initialized by this point.
* \returns 0 on success or -1 if initialisation failed
*/
int32_t SensorsStart(void)
{
// Start main task
xTaskCreate(SensorsTask, (signed char *)"Sensors", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &sensorsTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS);
#endif
return 0;
}
/**
* Create the module task.
* \returns 0 on success or -1 if initialization failed
*/
int32_t GPSPSystemModStart(void)
{
// Initialize vars
stackOverflow = STACKOVERFLOW_NONE;
mallocFailed = false;
// Create system task
xTaskCreate(gpspSystemTask, (const char *)"G-Sys", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_SYSTEM);
#endif
return 0;
}
/**
* @brief Start the module
*
* @return -1 if initialisation failed, 0 on success
*/
static int32_t RadioComBridgeStart(void)
{
if (data) {
// Check if this is the coordinator modem
data->isCoordinator = PIOS_RFM22B_IsCoordinator(PIOS_COM_RFM22B);
// Parse UAVTalk out of the link
data->parseUAVTalk = true;
// Configure our UAVObjects for updates.
UAVObjConnectQueue(UAVObjGetByID(RFM22BSTATUS_OBJID), data->uavtalkEventQueue,
EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->uavtalkEventQueue,
EV_UPDATED | EV_UPDATED_MANUAL);
if (data->isCoordinator) {
UAVObjConnectQueue(UAVObjGetByID(RFM22BRECEIVER_OBJID), data->radioEventQueue,
EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
} else {
UAVObjConnectQueue(UAVObjGetByID(RFM22BRECEIVER_OBJID), data->uavtalkEventQueue,
EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
}
if (data->isCoordinator) {
registerObject(RadioComBridgeStatsHandle());
}
// Configure the UAVObject callbacks
ObjectPersistenceConnectCallback(&objectPersistenceUpdatedCb);
// Start the primary tasks for receiving/sending UAVTalk packets from the GCS.
data->telemetryTxTaskHandle = PIOS_Thread_Create(telemetryTxTask, "telemetryTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
data->telemetryRxTaskHandle = PIOS_Thread_Create(telemetryRxTask, "telemetryRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
if (PIOS_PPM_RECEIVER != 0) {
data->PPMInputTaskHandle = PIOS_Thread_Create(PPMInputTask, "PPMInputTask",STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_PPMINPUT);
#endif
}
if (!data->parseUAVTalk) {
// If the user wants raw serial communication, we need to spawn another thread to handle it.
data->serialRxTaskHandle = PIOS_Thread_Create(serialRxTask, "serialRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_SERIALRX);
#endif
}
data->radioTxTaskHandle = PIOS_Thread_Create(radioTxTask, "radioTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
data->radioRxTaskHandle = PIOS_Thread_Create(radioRxTask, "radioRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
// Register the watchdog timers.
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMETRYTX);
PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMETRYRX);
PIOS_WDG_RegisterFlag(PIOS_WDG_RADIOTX);
PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORX);
#endif
return 0;
}
return -1;
}
/**
* Module initialization
*/
int32_t ManualControlInitialize()
{
/* Check the assumptions about uavobject enum's are correct */
if(!assumptions)
return -1;
// Start main task
xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
return 0;
}
/**
* Module initialization
*/
int32_t ManualControlInitialize()
{
// The following line compiles to nothing when the assumptions are correct
if (!assumptions)
return 1;
// In case the assumptions are incorrect, this module will not initialise and so will not function
// Start main task
xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
return 0;
}
/**
* Module initialization
*/
int32_t StabilizationStart()
{
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
ManualControlCommandConnectCallback(FlightModeSwitchUpdatedCb);
StabilizationBankConnectCallback(BankUpdatedCb);
StabilizationSettingsBank1ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank2ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank3ConnectCallback(SettingsBankUpdatedCb);
StabilizationDesiredConnectCallback(StabilizationDesiredUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
StabilizationDesiredUpdatedCb(StabilizationDesiredHandle());
FlightModeSwitchUpdatedCb(ManualControlCommandHandle());
BankUpdatedCb(StabilizationBankHandle());
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);
#endif
return 0;
}
/**
* Module initialization
*/
int32_t StabilizationStart()
{
// Initialize variables
// Create object queue
queue = PIOS_Queue_Create(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Listen for updates.
// AttitudeActualConnectQueue(queue);
GyrosConnectQueue(queue);
// Connect settings callback
MWRateSettingsConnectCallback(SettingsUpdatedCb);
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
TrimAnglesSettingsConnectCallback(SettingsUpdatedCb);
// Start main task
taskHandle = PIOS_Thread_Create(stabilizationTask, "Stabilization", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_STABILIZATION, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);
return 0;
}
