static int32_t comUsbBridgeStart(void)
{
if (module_enabled) {
// Start tasks
com2UsbBridgeTaskHandle = PIOS_Thread_Create(com2UsbBridgeTask, "Com2UsbBridge", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_COM2USBBRIDGE, com2UsbBridgeTaskHandle);
usb2ComBridgeTaskHandle = PIOS_Thread_Create(usb2ComBridgeTask, "Usb2ComBridge", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_USB2COMBRIDGE, usb2ComBridgeTaskHandle);
return 0;
}
return -1;
}
/**
* @brief Initialize the L3GD20 3-axis gyro sensor.
* @return 0 for success, -1 for failure to allocate, -2 for failure to get irq
*/
int32_t PIOS_L3GD20_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_l3gd20_cfg *cfg)
{
pios_l3gd20_dev = PIOS_L3GD20_alloc();
if (pios_l3gd20_dev == NULL)
return -1;
pios_l3gd20_dev->spi_id = spi_id;
pios_l3gd20_dev->slave_num = slave_num;
pios_l3gd20_dev->cfg = cfg;
/* Configure the L3GD20 Sensor */
if(PIOS_L3GD20_Config(cfg) != 0)
return -2;
pios_l3gd20_dev->threadp = PIOS_Thread_Create(
PIOS_L3GD20_Task, "pios_l3gd20", L3GD20_TASK_STACK, NULL, L3GD20_TASK_PRIORITY);
PIOS_Assert(pios_l3gd20_dev->threadp != NULL);
/* Set up EXTI */
PIOS_EXTI_Init(cfg->exti_cfg);
// An initial read is needed to get it running
struct pios_l3gd20_data data;
PIOS_L3GD20_ReadGyros(&data);
PIOS_SENSORS_Register(PIOS_SENSOR_GYRO, pios_l3gd20_dev->queue);
return 0;
}
/**
* Start the Logging module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t LoggingStart(void)
{
//Check if module is enabled or not
if (module_enabled == false) {
return -1;
}
// create logging queue
logging_queue = PIOS_Queue_Create(LOGGING_QUEUE_SIZE, sizeof(UAVObjEvent));
if (!logging_queue){
return -1;
}
// Create mutex
mutex = PIOS_Recursive_Mutex_Create();
if (mutex == NULL){
return -2;
}
// Start logging task
loggingTaskHandle = PIOS_Thread_Create(loggingTask, "Logging", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_LOGGING, loggingTaskHandle);
return 0;
}
/**
* Open UDP socket
*/
int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg)
{
pios_udp_dev * udp_dev = &pios_udp_devices[pios_udp_num_devices];
pios_udp_num_devices++;
/* initialize */
udp_dev->rx_in_cb = NULL;
udp_dev->tx_out_cb = NULL;
udp_dev->cfg=cfg;
/* assign socket */
udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
memset(&udp_dev->server,0,sizeof(udp_dev->server));
memset(&udp_dev->client,0,sizeof(udp_dev->client));
udp_dev->server.sin_family = AF_INET;
udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip);
udp_dev->server.sin_port = htons(udp_dev->cfg->port);
int res= bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server,sizeof(udp_dev->server));
/* Create transmit thread for this connection */
udp_dev->rxThread = PIOS_Thread_Create(
PIOS_TCP_RxTask, "pios_udp_rx", PIOS_THREAD_STACK_SIZE_MIN, udp_dev, PIOS_THREAD_PRIO_NORMAL);
printf("udp dev %i - socket %i opened - result %i\n",pios_udp_num_devices-1,udp_dev->socket,res);
*udp_id = pios_udp_num_devices-1;
return res;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t ExampleModPeriodicInitialize()
{
// Start main task
taskHandle = PIOS_Thread_Create(exampleTask, "ExamplePeriodic", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
return 0;
}
/**
* @brief Initialize the MPU9150 3-axis gyro sensor.
* @return 0 for success, -1 for failure to allocate, -2 for failure to get irq
*/
int32_t PIOS_MPU9150_Init(uint32_t i2c_id, uint8_t i2c_addr, const struct pios_mpu60x0_cfg * cfg)
{
dev = PIOS_MPU9150_alloc();
if (dev == NULL)
return -1;
dev->i2c_id = i2c_id;
dev->i2c_addr = i2c_addr;
dev->cfg = cfg;
/* Configure the MPU9150 Sensor */
if (PIOS_MPU9150_Config(cfg) != 0)
return -2;
/* Set up EXTI line */
PIOS_EXTI_Init(cfg->exti_cfg);
// Wait 5 ms for data ready interrupt and make sure it happens
// twice
if ((PIOS_Semaphore_Take(dev->data_ready_sema, 5) != true) ||
(PIOS_Semaphore_Take(dev->data_ready_sema, 5) != true)) {
return -10;
}
dev->TaskHandle = PIOS_Thread_Create(
PIOS_MPU9150_Task, "pios_mpu9150", MPU9150_TASK_STACK_BYTES, NULL, MPU9150_TASK_PRIORITY);
PIOS_Assert(dev->TaskHandle != NULL);
PIOS_SENSORS_Register(PIOS_SENSOR_ACCEL, dev->accel_queue);
PIOS_SENSORS_Register(PIOS_SENSOR_GYRO, dev->gyro_queue);
PIOS_SENSORS_Register(PIOS_SENSOR_MAG, dev->mag_queue);
return 0;
}
/**
* @brief Initialize the MPU6000 3-axis gyro sensor.
* @return 0 for success, -1 for failure
*/
int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu60x0_cfg *cfg)
{
pios_mpu6000_dev = PIOS_MPU6000_alloc();
if (pios_mpu6000_dev == NULL)
return -1;
pios_mpu6000_dev->spi_id = spi_id;
pios_mpu6000_dev->slave_num = slave_num;
pios_mpu6000_dev->cfg = cfg;
/* Configure the MPU6000 Sensor */
PIOS_SPI_SetClockSpeed(pios_mpu6000_dev->spi_id, 100000);
PIOS_MPU6000_Config(cfg);
PIOS_SPI_SetClockSpeed(pios_mpu6000_dev->spi_id, 3000000);
pios_mpu6000_dev->threadp = PIOS_Thread_Create(
PIOS_MPU6000_Task, "pios_mpu6000", MPU6000_TASK_STACK, NULL, MPU6000_TASK_PRIORITY);
PIOS_Assert(pios_mpu6000_dev->threadp != NULL);
/* Set up EXTI line */
PIOS_EXTI_Init(cfg->exti_cfg);
#if defined(PIOS_MPU6000_ACCEL)
PIOS_SENSORS_Register(PIOS_SENSOR_ACCEL, pios_mpu6000_dev->accel_queue);
#endif /* PIOS_MPU6000_ACCEL */
PIOS_SENSORS_Register(PIOS_SENSOR_GYRO, pios_mpu6000_dev->gyro_queue);
return 0;
}
/**
* @brief Initialize the HMC5883 magnetometer sensor.
* @return 0 on success
*/
int32_t PIOS_HMC5883_Init(uint32_t i2c_id, const struct pios_hmc5883_cfg *cfg)
{
dev = (struct hmc5883_dev *) PIOS_HMC5883_alloc();
if (dev == NULL)
return -1;
dev->cfg = cfg;
dev->i2c_id = i2c_id;
dev->orientation = cfg->Default_Orientation;
/* check if we are using an irq line */
if (cfg->exti_cfg != NULL) {
PIOS_EXTI_Init(cfg->exti_cfg);
dev->data_ready_sema = PIOS_Semaphore_Create();
PIOS_Assert(dev->data_ready_sema != NULL);
}
else {
dev->data_ready_sema = NULL;
}
if (PIOS_HMC5883_Config(cfg) != 0)
return -2;
PIOS_SENSORS_Register(PIOS_SENSOR_MAG, dev->queue);
dev->task = PIOS_Thread_Create(PIOS_HMC5883_Task, "pios_hmc5883", HMC5883_TASK_STACK_BYTES, NULL, HMC5883_TASK_PRIORITY);
PIOS_Assert(dev->task != NULL);
return 0;
}
/**
* Initialise the telemetry module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t OveroSyncStart(void)
{
overosync = (struct overosync *) PIOS_malloc(sizeof(*overosync));
if(overosync == NULL)
return -1;
overosync->buffer_lock = PIOS_Mutex_Create();
if(overosync->buffer_lock == NULL)
return -1;
overosync->active_transaction_id = 0;
overosync->loading_transaction_id = 0;
overosync->write_pointer = 0;
overosync->sent_bytes = 0;
overosync->framesync_error = 0;
// Process all registered objects and connect queue for updates
UAVObjIterate(®isterObject);
// Start telemetry tasks
overoSyncTaskHandle = PIOS_Thread_Create(overoSyncTask, "OveroSync", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_OVEROSYNC, overoSyncTaskHandle);
return 0;
}
/**
* Tau Labs Main function:
*
* Initialize PiOS<BR>
* Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR>
* Start FreeRTOS Scheduler (vTaskStartScheduler)<BR>
* If something goes wrong, blink LED1 and LED2 every 100ms
*
*/
int main()
{
/* NOTE: Do NOT modify the following start-up sequence */
/* Any new initialization functions should be added in OpenPilotInit() */
PIOS_heap_initialize_blocks();
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* For Revolution we use a FreeRTOS task to bring up the system so we can */
/* always rely on FreeRTOS primitive */
initTaskHandle = PIOS_Thread_Create(initTask, "init", INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY);
PIOS_Assert(initTaskHandle != NULL);
/* Start the FreeRTOS scheduler */
vTaskStartScheduler();
/* If all is well we will never reach here as the scheduler will now be running. */
/* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */
PIOS_LED_Off(PIOS_LED_HEARTBEAT); \
for(;;) { \
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \
PIOS_DELAY_WaitmS(100); \
};
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;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t FixedWingPathFollowerStart()
{
if (module_enabled) {
// Start main task
pathfollowerTaskHandle = PIOS_Thread_Create(pathfollowerTask, "PathFollower", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
}
return 0;
}
/**
* Create the module task.
* \returns 0 on success or -1 if initialization failed
*/
int32_t SystemModStart(void)
{
// Initialize vars
stackOverflow = false;
// Create system task
systemTaskHandle = PIOS_Thread_Create(systemTask, "System", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
// Register task
TaskMonitorAdd(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
return 0;
}
/**
* Initialise the module
* \return -1 if initialisation failed
* \return 0 on success
*/
static int32_t uavoMavlinkBridgeStart(void) {
if (module_enabled) {
// Start tasks
uavoMavlinkBridgeTaskHandle = PIOS_Thread_Create(
uavoMavlinkBridgeTask, "uavoMavlinkBridge", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_UAVOMAVLINKBRIDGE,
uavoMavlinkBridgeTaskHandle);
return 0;
}
return -1;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AltitudeHoldStart()
{
// Start main task if it is enabled
if (module_enabled) {
altitudeHoldTaskHandle = PIOS_Thread_Create(altitudeHoldTask, "AltitudeHold", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_ALTITUDEHOLD, altitudeHoldTaskHandle);
return 0;
}
return -1;
}
/**
* 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) {
taskHandle = PIOS_Thread_Create(AutotuneTask, "Autotune", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_AUTOTUNE, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_AUTOTUNE);
}
return 0;
}
/**
* Initialise Servos
*/
int32_t PIOS_Brushless_Init(const struct pios_brushless_cfg * cfg)
{
uintptr_t tim_id;
if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, NULL, 0)) {
return -1;
}
/* Store away the requested configuration */
brushless_cfg = cfg;
/* Configure the channels to be in output compare mode */
for (uint8_t i = 0; i < cfg->num_channels; i++) {
const struct pios_tim_channel * chan = &cfg->channels[i];
/* Set up for output compare function */
switch(chan->timer_chan) {
case TIM_Channel_1:
TIM_OC1Init(chan->timer, (TIM_OCInitTypeDef*)&cfg->tim_oc_init);
TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_2:
TIM_OC2Init(chan->timer, (TIM_OCInitTypeDef*)&cfg->tim_oc_init);
TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_3:
TIM_OC3Init(chan->timer, (TIM_OCInitTypeDef*)&cfg->tim_oc_init);
TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_4:
TIM_OC4Init(chan->timer, (TIM_OCInitTypeDef*)&cfg->tim_oc_init);
TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
}
TIM_ARRPreloadConfig(chan->timer, ENABLE);
TIM_CtrlPWMOutputs(chan->timer, ENABLE);
TIM_Cmd(chan->timer, ENABLE);
}
for (uint8_t i = 0 ; i < NUM_BGC_CHANNELS; i++) {
// Enable the available enable lines
if (cfg->enables[i].gpio) {
GPIO_Init(cfg->enables[i].gpio, (GPIO_InitTypeDef *) &cfg->enables[i].init);
GPIO_SetBits(cfg->enables[i].gpio, cfg->enables[i].init.GPIO_Pin);
}
}
// Start main task
taskHandle = PIOS_Thread_Create(
PIOS_BRUSHLESS_Task, "pios_brushless", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
return 0;
}
/**
* Initialise the telemetry module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t TelemetryStart(void)
{
// Process all registered objects and connect queue for updates
UAVObjIterate(®isterObject);
// Listen to objects of interest
GCSTelemetryStatsConnectQueue(priorityQueue);
// Start telemetry tasks
telemetryTxTaskHandle = PIOS_Thread_Create(telemetryTxTask, "TelTx", STACK_SIZE_BYTES, NULL, TASK_PRIORITY_TX);
telemetryRxTaskHandle = PIOS_Thread_Create(telemetryRxTask, "TelRx", STACK_SIZE_BYTES, NULL, TASK_PRIORITY_RX);
TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle);
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
telemetryTxPriTaskHandle = PIOS_Thread_Create(telemetryTxPriTask, "TelPriTx", STACK_SIZE_BYTES, NULL, TASK_PRIORITY_TXPRI);
TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTXPRI, telemetryTxPriTaskHandle);
#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;
}
/**
* Start the module
* \return -1 if start failed
* \return 0 on success
*/
static int32_t uavoTaranisStart(void)
{
if (module_enabled) {
// Start tasks
uavoTaranisTaskHandle = PIOS_Thread_Create(
uavoTaranisTask, "uavoFrSKYSensorHubBridge",
STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_UAVOFRSKYSBRIDGE,
uavoTaranisTaskHandle);
return 0;
}
return -1;
}
/**
* Module initialization
*/
int32_t PathPlannerStart()
{
if(module_enabled) {
taskHandle = NULL;
// Start VM thread
taskHandle = PIOS_Thread_Create(pathPlannerTask, "PathPlanner", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_PATHPLANNER, taskHandle);
return 0;
}
return -1;
}
int32_t GPSStart(void)
{
if (module_enabled) {
if (gpsPort) {
// Start gps task
gpsTaskHandle = PIOS_Thread_Create(gpsTask, "GPS", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_GPS, gpsTaskHandle);
return 0;
}
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
}
return -1;
}
/**
* Start the Logging module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t LoggingStart(void)
{
//Check if module is enabled or not
if (module_enabled == false) {
return -1;
}
// Start logging task
loggingTaskHandle = PIOS_Thread_Create(loggingTask, "Logging", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_LOGGING, loggingTaskHandle);
return 0;
}
/**
* Start the UAVORelay module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t GimbalControlStart(void)
{
//Check if module is enabled or not
if (module_enabled == false) {
return -1;
}
// Start relay task
gimbalControlTaskHandle = PIOS_Thread_Create(
gimbalControlTask, "GimbalControl", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_UAVORELAY, gimbalControlTaskHandle);
return 0;
}
/**
* Start the UAVORelay module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t UAVORelayStart(void)
{
//Check if module is enabled or not
if (module_enabled == false) {
return -1;
}
// Register objects to relay
if (CameraDesiredHandle())
register_object(CameraDesiredHandle());
// Start relay task
uavoRelayTaskHandle = PIOS_Thread_Create(
uavoRelayTask, "UAVORelay", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_UAVORELAY, uavoRelayTaskHandle);
return 0;
}
int main(int argc, char *argv[]) {
PIOS_SYS_Args(argc, argv);
#else
int main()
{
#endif
/* NOTE: Do NOT modify the following start-up sequence */
/* Any new initialization functions should be added in OpenPilotInit() */
PIOS_heap_initialize_blocks();
#if defined(PIOS_INCLUDE_CHIBIOS)
halInit();
chSysInit();
boardInit();
#endif /* defined(PIOS_INCLUDE_CHIBIOS) */
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* For Revolution we use a FreeRTOS task to bring up the system so we can */
/* always rely on FreeRTOS primitive */
initTaskHandle = PIOS_Thread_Create(initTask, "init", INIT_TASK_STACK, NULL, INIT_TASK_PRIORITY);
PIOS_Assert(initTaskHandle != NULL);
#if defined(PIOS_INCLUDE_FREERTOS)
/* Start the FreeRTOS scheduler */
vTaskStartScheduler();
/* If all is well we will never reach here as the scheduler will now be running. */
/* Do some PIOS_LED_HEARTBEAT to user that something bad just happened */
PIOS_LED_Off(PIOS_LED_HEARTBEAT); \
for(;;) { \
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); \
PIOS_DELAY_WaitmS(100); \
};
#elif defined(PIOS_INCLUDE_CHIBIOS)
PIOS_Thread_Sleep(PIOS_THREAD_TIMEOUT_MAX);
#endif /* defined(PIOS_INCLUDE_CHIBIOS) */
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;
}
/**
* @brief Initialize the PX4Flow optical flow sensor.
* @return 0 on success
*/
int32_t PIOS_PX4Flow_Init(const struct pios_px4flow_cfg *cfg, const uint32_t i2c_id)
{
dev = (struct px4flow_dev *) PIOS_PX4Flow_alloc();
if (dev == NULL)
return -1;
dev->cfg = cfg;
dev->i2c_id = i2c_id;
PIOS_PX4Flow_SetRotation(cfg->rotation);
if (PIOS_PX4Flow_Config(cfg) != 0)
return -2;
PIOS_SENSORS_Register(PIOS_SENSOR_OPTICAL_FLOW, dev->optical_flow_queue);
PIOS_SENSORS_Register(PIOS_SENSOR_RANGEFINDER, dev->rangefinder_queue);
dev->task = PIOS_Thread_Create(PIOS_PX4Flow_Task, "pios_px4flow", PX4FLOW_TASK_STACK_BYTES, NULL, PX4FLOW_TASK_PRIORITY);
PIOS_Assert(dev->task != NULL);
return 0;
}
/**
* Start the overo sync module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t OveroSyncStart(void)
{
//Check if module is enabled or not
if (module_enabled == false) {
return -1;
}
overosync = (struct overosync *) PIOS_malloc(sizeof(*overosync));
if(overosync == NULL)
return -1;
overosync->sent_bytes = 0;
// Process all registered objects and connect queue for updates
UAVObjIterate(®ister_object);
// Start telemetry tasks
overoSyncTaskHandle = PIOS_Thread_Create(overoSyncTask, "OveroSync", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
TaskMonitorAdd(TASKINFO_RUNNING_OVEROSYNC, overoSyncTaskHandle);
return 0;
}
