/**
* @brief Allocate a new device
*/
static struct mpu6000_dev *PIOS_MPU6000_alloc(void)
{
struct mpu6000_dev *mpu6000_dev;
mpu6000_dev = (struct mpu6000_dev *)PIOS_malloc(sizeof(*mpu6000_dev));
if (!mpu6000_dev) return (NULL);
mpu6000_dev->magic = PIOS_MPU6000_DEV_MAGIC;
mpu6000_dev->configured = false;
#if defined(PIOS_MPU6000_ACCEL)
mpu6000_dev->accel_queue = PIOS_Queue_Create(PIOS_MPU6000_MAX_QUEUESIZE, sizeof(struct pios_sensor_accel_data));
if (mpu6000_dev->accel_queue == NULL) {
PIOS_free(mpu6000_dev);
return NULL;
}
#endif /* PIOS_MPU6000_ACCEL */
mpu6000_dev->gyro_queue = PIOS_Queue_Create(PIOS_MPU6000_MAX_QUEUESIZE, sizeof(struct pios_sensor_gyro_data));
if (mpu6000_dev->gyro_queue == NULL) {
PIOS_free(mpu6000_dev);
return NULL;
}
return mpu6000_dev;
}
/**
* @brief Allocate a new device
*/
static struct mpu9150_dev * PIOS_MPU9150_alloc(void)
{
struct mpu9150_dev * mpu9150_dev;
mpu9150_dev = (struct mpu9150_dev *)PIOS_malloc(sizeof(*mpu9150_dev));
if (!mpu9150_dev) return (NULL);
mpu9150_dev->magic = PIOS_MPU9150_DEV_MAGIC;
mpu9150_dev->accel_queue = PIOS_Queue_Create(PIOS_MPU9150_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_gyro_data));
if (mpu9150_dev->accel_queue == NULL) {
PIOS_free(mpu9150_dev);
return NULL;
}
mpu9150_dev->gyro_queue = PIOS_Queue_Create(PIOS_MPU9150_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_gyro_data));
if (mpu9150_dev->gyro_queue == NULL) {
PIOS_free(mpu9150_dev);
return NULL;
}
mpu9150_dev->mag_queue = PIOS_Queue_Create(PIOS_MPU9150_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_mag_data));
if (mpu9150_dev->mag_queue == NULL) {
PIOS_free(mpu9150_dev);
return NULL;
}
mpu9150_dev->data_ready_sema = PIOS_Semaphore_Create();
if (mpu9150_dev->data_ready_sema == NULL) {
PIOS_free(mpu9150_dev);
return NULL;
}
return mpu9150_dev;
}
/**
*
* @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;
}
/**
* @brief Allocate a new device
*/
static struct l3gd20_dev *PIOS_L3GD20_alloc(void)
{
struct l3gd20_dev *l3gd20_dev;
l3gd20_dev = (struct l3gd20_dev *)PIOS_malloc(sizeof(*l3gd20_dev));
if (!l3gd20_dev) return (NULL);
l3gd20_dev->magic = PIOS_L3GD20_DEV_MAGIC;
l3gd20_dev->configured = false;
l3gd20_dev->queue = PIOS_Queue_Create(PIOS_L3GD20_QUEUESIZE, sizeof(struct pios_sensor_gyro_data));
if (l3gd20_dev->queue == NULL) {
PIOS_free(l3gd20_dev);
return NULL;
}
l3gd20_dev->data_ready_sema = PIOS_Semaphore_Create();
if (l3gd20_dev->data_ready_sema == NULL) {
PIOS_free(l3gd20_dev);
return NULL;
}
return l3gd20_dev;
}
/**
* @brief Allocate a new device
*/
static struct mpu9250_dev *PIOS_MPU9250_alloc(const struct pios_mpu9250_cfg *cfg)
{
struct mpu9250_dev *mpu9250_dev;
mpu9250_dev = (struct mpu9250_dev *)PIOS_malloc(sizeof(*mpu9250_dev));
if (!mpu9250_dev)
return NULL;
mpu9250_dev->magic = PIOS_MPU9250_DEV_MAGIC;
mpu9250_dev->accel_queue = PIOS_Queue_Create(PIOS_MPU9250_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_accel_data));
if (mpu9250_dev->accel_queue == NULL) {
PIOS_free(mpu9250_dev);
return NULL;
}
mpu9250_dev->gyro_queue = PIOS_Queue_Create(PIOS_MPU9250_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_gyro_data));
if (mpu9250_dev->gyro_queue == NULL) {
PIOS_Queue_Delete(dev->accel_queue);
PIOS_free(mpu9250_dev);
return NULL;
}
if (cfg->use_magnetometer) {
mpu9250_dev->mag_queue = PIOS_Queue_Create(PIOS_MPU9250_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_mag_data));
if (mpu9250_dev->mag_queue == NULL) {
PIOS_Queue_Delete(dev->accel_queue);
PIOS_Queue_Delete(dev->gyro_queue);
PIOS_free(mpu9250_dev);
return NULL;
}
}
mpu9250_dev->data_ready_sema = PIOS_Semaphore_Create();
if (mpu9250_dev->data_ready_sema == NULL) {
PIOS_Queue_Delete(dev->accel_queue);
PIOS_Queue_Delete(dev->gyro_queue);
if (cfg->use_magnetometer)
PIOS_Queue_Delete(dev->mag_queue);
PIOS_free(mpu9250_dev);
return NULL;
}
return mpu9250_dev;
}
/**
* @brief Allocate a new device
*/
static struct hmc5883_dev * PIOS_HMC5883_alloc(void)
{
struct hmc5883_dev *hmc5883_dev;
hmc5883_dev = (struct hmc5883_dev *)PIOS_malloc(sizeof(*hmc5883_dev));
if (!hmc5883_dev) return (NULL);
hmc5883_dev->magic = PIOS_HMC5883_DEV_MAGIC;
hmc5883_dev->queue = PIOS_Queue_Create(PIOS_HMC5883_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_mag_data));
if (hmc5883_dev->queue == NULL) {
PIOS_free(hmc5883_dev);
return NULL;
}
return hmc5883_dev;
}
/**
*
* @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;
}
/**
*
* @brief Creates a queue.
*
* @returns instance of @p struct pios_queue or NULL on failure
*
*/
struct pios_queue *PIOS_Queue_Create(size_t queue_length, size_t item_size)
{
struct pios_queue *queuep = PIOS_malloc(sizeof(struct pios_queue));
if (queuep == NULL)
return NULL;
queuep->queue_handle = (uintptr_t)NULL;
if ((queuep->queue_handle = (uintptr_t)xQueueCreate(queue_length, item_size)) == (uintptr_t)NULL)
{
PIOS_free(queuep);
return NULL;
}
return queuep;
}
/**
* @brief Allocate a new device
*/
static struct px4flow_dev * PIOS_PX4Flow_alloc(void)
{
struct px4flow_dev *px4flow_dev;
px4flow_dev = (struct px4flow_dev *)PIOS_malloc(sizeof(*px4flow_dev));
if (!px4flow_dev) return (NULL);
px4flow_dev->magic = PIOS_PX4FLOW_DEV_MAGIC;
px4flow_dev->optical_flow_queue = PIOS_Queue_Create(PIOS_PX4FLOW_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_optical_flow_data));
px4flow_dev->rangefinder_queue = PIOS_Queue_Create(PIOS_PX4FLOW_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_rangefinder_data));
if (px4flow_dev->optical_flow_queue == NULL || px4flow_dev->rangefinder_queue == NULL) {
PIOS_free(px4flow_dev);
return NULL;
}
return px4flow_dev;
}
/**
*
* @brief Creates a queue.
*
* @returns instance of @p struct pios_queue or NULL on failure
*
*/
struct pios_queue *PIOS_Queue_Create(size_t queue_length, size_t item_size)
{
struct pios_queue *queuep = PIOS_malloc(sizeof(struct pios_queue));
if (queuep == NULL)
return NULL;
/* Create the memory pool. */
queuep->mpb = PIOS_malloc(item_size * (queue_length + PIOS_QUEUE_MAX_WAITERS));
if (queuep->mpb == NULL) {
PIOS_free(queuep);
return NULL;
}
chPoolInit(&queuep->mp, item_size, NULL);
chPoolLoadArray(&queuep->mp, queuep->mpb, queue_length + PIOS_QUEUE_MAX_WAITERS);
/* Create the mailbox. */
msg_t *mb_buf = PIOS_malloc(sizeof(msg_t) * queue_length);
chMBInit(&queuep->mb, mb_buf, queue_length);
return queuep;
}
/**
*
* @brief Destroys an instance of @p struct pios_queue
*
* @param[in] queuep pointer to instance of @p struct pios_queue
*
*/
void PIOS_Queue_Delete(struct pios_queue *queuep)
{
vQueueDelete((xQueueHandle)queuep->queue_handle);
PIOS_free(queuep);
}
/**
*
* @brief Destroys an instance of @p struct pios_queue
*
* @param[in] queuep pointer to instance of @p struct pios_queue
*
*/
void PIOS_Queue_Delete(struct pios_queue *queuep)
{
PIOS_free(queuep->mpb);
PIOS_free(queuep);
}
/**
* @brief Initialize the flash object setting FS
* @return 0 if success, -1 if failure
*/
int32_t PIOS_STREAMFS_Init(uintptr_t *fs_id, const struct streamfs_cfg *cfg, enum pios_flash_partition_labels partition_label)
{
PIOS_Assert(cfg);
/* Find the partition id for the requested partition label */
uintptr_t partition_id;
if (PIOS_FLASH_find_partition_id(partition_label, &partition_id) != 0) {
return -1;
}
/* Query the total partition size */
uint32_t partition_size;
if (PIOS_FLASH_get_partition_size(partition_id, &partition_size) != 0) {
return -1;
}
/* sector_size must exactly divide the partition size */
PIOS_Assert((partition_size % cfg->arena_size) == 0);
/* sector_size must exceed write_size */
PIOS_Assert(cfg->arena_size > cfg->write_size);
int8_t rc;
struct streamfs_state *streamfs;
streamfs = (struct streamfs_state *) streamfs_alloc();
if (!streamfs) {
rc = -1;
goto out_exit;
}
streamfs->com_buffer = (uint8_t *)PIOS_malloc(cfg->write_size);
if (!streamfs->com_buffer) {
PIOS_free(streamfs);
return -1;
}
/* Bind configuration parameters to this filesystem instance */
streamfs->cfg = cfg; /* filesystem configuration */
streamfs->partition_id = partition_id; /* underlying partition */
streamfs->partition_size = partition_size; /* size of underlying partition */
streamfs->partition_arenas = partition_size / cfg->arena_size;
streamfs->file_open_writing = false;
streamfs->file_open_reading = false;
streamfs->active_file_id = 0;
streamfs->active_file_arena = 0;
streamfs->active_file_arena_offset = 0;
if (PIOS_FLASH_start_transaction(streamfs->partition_id) != 0) {
rc = -1;
goto out_exit;
}
// TODO: find the first sector after the last file
// TODO: validate that the partition is valid for streaming (magic?)
// Scan filesystem contents
streamfs_scan_filesystem(streamfs);
rc = 0;
*fs_id = (uintptr_t) streamfs;
//out_end_trans:
PIOS_FLASH_end_transaction(streamfs->partition_id);
out_exit:
return rc;
}
static void streamfs_free(struct streamfs_state *streamfs)
{
/* Invalidate the magic */
streamfs->magic = ~PIOS_FLASHFS_STREAMFS_DEV_MAGIC;
PIOS_free(streamfs);
}