/**
* Initialise the module
* \return -1 if initialisation failed
* \return 0 on success
*/
static int32_t uavoMavlinkBridgeInitialize(void) {
mavlink_port = PIOS_COM_MAVLINK;
uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_state);
if (mavlink_port
&& (module_state[MODULESETTINGS_ADMINSTATE_UAVOMAVLINKBRIDGE]
== MODULESETTINGS_ADMINSTATE_ENABLED)) {
updateSettings();
mav_msg = PIOS_malloc(sizeof(*mav_msg));
stream_ticks = PIOS_malloc_no_dma(MAXSTREAMS);
if (mav_msg && stream_ticks) {
for (int x = 0; x < MAXSTREAMS; ++x) {
stream_ticks[x] = (TASK_RATE_HZ / mav_rates[x]);
}
module_enabled = true;
}
}
return 0;
}
/**
*
* @brief Creates a thread.
*
* @param[in] fp pointer to thread function
* @param[in] namep pointer to thread name
* @param[in] stack_bytes stack size in bytes
* @param[in] argp pointer to argument which will be passed to thread function
* @param[in] prio thread priority
*
* @returns instance of @p struct pios_thread or NULL on failure
*
*/
struct pios_thread *PIOS_Thread_Create(void (*fp)(void *), const char *namep, size_t stack_bytes, void *argp, enum pios_thread_prio_e prio)
{
struct pios_thread *thread = PIOS_malloc_no_dma(sizeof(struct pios_thread));
if (thread == NULL)
return NULL;
#ifdef SIM_POSIX
if (stack_bytes < PIOS_THREAD_STACK_SIZE_MIN) {
stack_bytes = PIOS_THREAD_STACK_SIZE_MIN;
}
#endif
// Use special functions to ensure ChibiOS stack requirements
stack_bytes = ceil_size(stack_bytes);
uint8_t *wap = align8_alloc(stack_bytes);
if (wap == NULL)
{
PIOS_free(thread);
return NULL;
}
thread->threadp = chThdCreateStatic(wap, stack_bytes, prio, (msg_t (*)(void *))fp, argp);
if (thread->threadp == NULL)
{
PIOS_free(thread);
PIOS_free(wap);
return NULL;
}
#if CH_USE_REGISTRY
thread->threadp->p_name = namep;
#endif /* CH_USE_REGISTRY */
return thread;
}
static struct streamfs_state *streamfs_alloc(void)
{
struct streamfs_state *streamfs;
streamfs = (struct streamfs_state *)PIOS_malloc_no_dma(sizeof(*streamfs));
if (!streamfs) return (NULL);
streamfs->magic = PIOS_FLASHFS_STREAMFS_DEV_MAGIC;
return(streamfs);
}
/**
*
* @brief Creates a thread.
*
* @param[in] fp pointer to thread function
* @param[in] namep pointer to thread name
* @param[in] stack_bytes stack size in bytes
* @param[in] argp pointer to argument which will be passed to thread function
* @param[in] prio thread priority
*
* @returns instance of @p struct pios_thread or NULL on failure
*
*/
struct pios_thread *PIOS_Thread_Create(void (*fp)(void *), const char *namep, size_t stack_bytes, void *argp, enum pios_thread_prio_e prio)
{
struct pios_thread *thread = PIOS_malloc_no_dma(sizeof(struct pios_thread));
if (thread == NULL)
return NULL;
thread->task_handle = (uintptr_t)NULL;
if (xTaskCreate(fp, (signed char*)namep, stack_bytes / 4, argp, prio, (xTaskHandle*)&thread->task_handle) != pdPASS)
{
PIOS_free(thread);
return NULL;
}
return thread;
}
/**
* Initialize the UAVTalk library
* \param[in] connection UAVTalkConnection to be used
* \param[in] outputStream Function pointer that is called to send a data buffer
* \return 0 Success
* \return -1 Failure
*/
UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream)
{
// allocate object
UAVTalkConnectionData * connection = PIOS_malloc_no_dma(sizeof(UAVTalkConnectionData));
if (!connection) return 0;
connection->canari = UAVTALK_CANARI;
connection->iproc.rxPacketLength = 0;
connection->iproc.state = UAVTALK_STATE_SYNC;
connection->outStream = outputStream;
connection->lock = PIOS_Recursive_Mutex_Create();
PIOS_Assert(connection->lock != NULL);
connection->transLock = PIOS_Recursive_Mutex_Create();
PIOS_Assert(connection->transLock != NULL);
// allocate buffers
connection->rxBuffer = PIOS_malloc(UAVTALK_MAX_PACKET_LENGTH);
if (!connection->rxBuffer) return 0;
connection->txBuffer = PIOS_malloc(UAVTALK_MAX_PACKET_LENGTH);
if (!connection->txBuffer) return 0;
connection->respSema = PIOS_Semaphore_Create();
PIOS_Semaphore_Take(connection->respSema, 0); // reset to zero
UAVTalkResetStats( (UAVTalkConnection) connection );
return (UAVTalkConnection) connection;
}
