Beispiel #1
0
/**
 * @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;
}
Beispiel #2
0
/**
 * @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;
}
Beispiel #3
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;
}
Beispiel #4
0
/**
 * @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;
}
Beispiel #5
0
/**
 * @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;
}
Beispiel #6
0
/**
 * @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;
}
Beispiel #7
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;

	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;
}
Beispiel #8
0
/**
 *
 * @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;
}
Beispiel #9
0
/**
 * @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;
}
Beispiel #10
0
/**
 *
 * @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;
}
Beispiel #11
0
/**
 *
 * @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);
}
Beispiel #12
0
/**
 *
 * @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);
}
Beispiel #13
0
/**
 * @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;
}
Beispiel #14
0
static void streamfs_free(struct streamfs_state *streamfs)
{
    /* Invalidate the magic */
    streamfs->magic = ~PIOS_FLASHFS_STREAMFS_DEV_MAGIC;
    PIOS_free(streamfs);
}