static void PIOS_Board_configure_com (const struct pios_usart_cfg *usart_port_cfg, size_t rx_buf_len, size_t tx_buf_len,
const struct pios_com_driver *com_driver, uintptr_t *pios_com_id)
{
uintptr_t pios_usart_id;
if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer;
if (rx_buf_len > 0) {
rx_buffer = (uint8_t *) PIOS_malloc(rx_buf_len);
PIOS_Assert(rx_buffer);
} else {
rx_buffer = NULL;
}
uint8_t * tx_buffer;
if (tx_buf_len > 0) {
tx_buffer = (uint8_t *) PIOS_malloc(tx_buf_len);
PIOS_Assert(tx_buffer);
} else {
tx_buffer = NULL;
}
if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id,
rx_buffer, rx_buf_len,
tx_buffer, tx_buf_len)) {
PIOS_Assert(0);
}
}
/**
* Initialise the gps module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t GPSInitialize(void)
{
gpsPort = PIOS_COM_GPS;
uint8_t gpsProtocol;
#ifdef MODULE_GPS_BUILTIN
module_enabled = true;
#else
uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_state);
if (module_state[MODULESETTINGS_ADMINSTATE_GPS] == MODULESETTINGS_ADMINSTATE_ENABLED) {
module_enabled = true;
} else {
module_enabled = false;
}
#endif
// These things are only conditional on small F1 targets.
// Expected to be always present otherwise.
#ifdef SMALLF1
if (gpsPort && module_enabled) {
#endif
GPSPositionInitialize();
GPSVelocityInitialize();
#if !defined(PIOS_GPS_MINIMAL)
GPSTimeInitialize();
GPSSatellitesInitialize();
HomeLocationInitialize();
UBloxInfoInitialize();
#endif
#if defined(PIOS_GPS_PROVIDES_AIRSPEED)
AirspeedActualInitialize();
#endif
updateSettings();
#ifdef SMALLF1
}
#endif
if (gpsPort && module_enabled) {
ModuleSettingsGPSDataProtocolGet(&gpsProtocol);
switch (gpsProtocol) {
case MODULESETTINGS_GPSDATAPROTOCOL_NMEA:
gps_rx_buffer = PIOS_malloc(NMEA_MAX_PACKET_LENGTH);
break;
case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
gps_rx_buffer = PIOS_malloc(sizeof(struct UBXPacket));
break;
default:
gps_rx_buffer = NULL;
}
PIOS_Assert(gps_rx_buffer);
return 0;
}
return -1;
}
/**
* @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 mpu6050_dev *PIOS_MPU6050_alloc(void)
{
struct mpu6050_dev *mpu6050_dev;
mpu6050_dev = (struct mpu6050_dev *)PIOS_malloc(sizeof(*mpu6050_dev));
if (!mpu6050_dev) return (NULL);
mpu6050_dev->magic = PIOS_MPU6050_DEV_MAGIC;
#if defined(PIOS_MPU6050_ACCEL)
mpu6050_dev->accel_queue = xQueueCreate(PIOS_MPU6050_MAX_QUEUESIZE, sizeof(struct pios_sensor_accel_data));
if (mpu6050_dev->accel_queue == NULL) {
vPortFree(mpu6050_dev);
return NULL;
}
#endif /* PIOS_MPU6050_ACCEL */
mpu6050_dev->gyro_queue = xQueueCreate(PIOS_MPU6050_MAX_QUEUESIZE, sizeof(struct pios_sensor_gyro_data));
if (mpu6050_dev->gyro_queue == NULL) {
vPortFree(mpu6050_dev);
return NULL;
}
mpu6050_dev->data_ready_sema = xSemaphoreCreateMutex();
if (mpu6050_dev->data_ready_sema == NULL) {
vPortFree(mpu6050_dev);
return NULL;
}
return mpu6050_dev;
}
/**
* @brief Initialise the module
*
* @return -1 if initialisation failed on success
*/
static int32_t RadioComBridgeInitialize(void)
{
// allocate and initialize the static data storage only if module is enabled
data =
(RadioComBridgeData *) PIOS_malloc(sizeof(RadioComBridgeData));
if (!data) {
return -1;
}
// Initialize the UAVObjects that we use
RFM22BStatusInitialize();
ObjectPersistenceInitialize();
RFM22BReceiverInitialize();
RadioComBridgeStatsInitialize();
// Initialise UAVTalk
data->telemUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler);
data->radioUAVTalkCon = UAVTalkInitialize(&RadioSendHandler);
// Initialize the queues.
data->uavtalkEventQueue = PIOS_Queue_Create(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
data->radioEventQueue = PIOS_Queue_Create(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
// Initialize the statistics.
data->telemetryTxRetries = 0;
data->radioTxRetries = 0;
data->parseUAVTalk = true;
return 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;
}
/**
* 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;
}
/**
* 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;
}
/**
* @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;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t GeofenceInitialize(void)
{
bool module_enabled = false;
#ifdef MODULE_Geofence_BUILTIN
module_enabled = true;
#else
uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_state);
if (module_state[MODULESETTINGS_ADMINSTATE_GEOFENCE] == MODULESETTINGS_ADMINSTATE_ENABLED) {
module_enabled = true;
} else {
module_enabled = false;
}
#endif
if (module_enabled) {
GeoFenceSettingsInitialize();
// allocate and initialize the static data storage only if module is enabled
geofenceSettings = (GeoFenceSettingsData *) PIOS_malloc(sizeof(GeoFenceSettingsData));
if (geofenceSettings == NULL) {
module_enabled = false;
return -1;
}
GeoFenceSettingsConnectCallback(settingsUpdated);
settingsUpdated(NULL, NULL, NULL, 0);
return 0;
}
return -1;
}
static struct pios_usb_dev * PIOS_USB_alloc(void)
{
struct pios_usb_dev * usb_dev;
usb_dev = (struct pios_usb_dev *)PIOS_malloc(sizeof(*usb_dev));
if (!usb_dev) return(NULL);
usb_dev->magic = PIOS_USB_DEV_MAGIC;
return(usb_dev);
}
static struct pios_pwm_dev * PIOS_PWM_alloc(void)
{
struct pios_pwm_dev * pwm_dev;
pwm_dev = (struct pios_pwm_dev *)PIOS_malloc(sizeof(*pwm_dev));
if (!pwm_dev) return(NULL);
pwm_dev->magic = PIOS_PWM_DEV_MAGIC;
return(pwm_dev);
}
static struct pios_rcvr_dev * PIOS_RCVR_alloc(void)
{
struct pios_rcvr_dev * rcvr_dev;
rcvr_dev = (struct pios_rcvr_dev *)PIOS_malloc(sizeof(*rcvr_dev));
if (!rcvr_dev) return (NULL);
rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC;
return(rcvr_dev);
}
static struct pios_hsum_dev *PIOS_HSUM_Alloc(void)
{
struct pios_hsum_dev *hsum_dev;
hsum_dev = (struct pios_hsum_dev *)PIOS_malloc(sizeof(*hsum_dev));
if (!hsum_dev)
return NULL;
hsum_dev->magic = PIOS_HSUM_DEV_MAGIC;
return hsum_dev;
}
static struct pios_internal_flash_dev *PIOS_Flash_Internal_alloc(void)
{
struct pios_internal_flash_dev *flash_dev;
flash_dev = (struct pios_internal_flash_dev *)PIOS_malloc(sizeof(*flash_dev));
if (!flash_dev) return (NULL);
flash_dev->magic = PIOS_INTERNAL_FLASH_DEV_MAGIC;
return(flash_dev);
}
/**
*
* @brief Creates a non recursive mutex.
*
* @returns instance of @p struct pios_mutex or NULL on failure
*
*/
struct pios_mutex *PIOS_Mutex_Create(void)
{
struct pios_mutex *mtx = PIOS_malloc(sizeof(struct pios_mutex));
if (mtx == NULL)
return NULL;
mtx->mtx_handle = (uintptr_t)xSemaphoreCreateMutex();
return mtx;
}
/**
*
* @brief Creates a non recursive mutex.
*
* @returns instance of @p struct pios_mutex or NULL on failure
*
*/
struct pios_mutex *PIOS_Mutex_Create(void)
{
struct pios_mutex *mtx = PIOS_malloc(sizeof(struct pios_mutex));
if (mtx == NULL)
return NULL;
chMtxInit(&mtx->mtx);
return mtx;
}
static struct pios_usart_dev * PIOS_USART_alloc(void)
{
struct pios_usart_dev * usart_dev;
usart_dev = (struct pios_usart_dev *)PIOS_malloc(sizeof(*usart_dev));
if (!usart_dev) return(NULL);
memset(usart_dev, 0, sizeof(*usart_dev));
usart_dev->magic = PIOS_USART_DEV_MAGIC;
return(usart_dev);
}
/**
*
* @brief Creates a recursive mutex.
*
* @returns instance of @p struct pios_recursive mutex or NULL on failure
*
*/
struct pios_recursive_mutex *PIOS_Recursive_Mutex_Create(void)
{
struct pios_recursive_mutex *mtx = PIOS_malloc(sizeof(struct pios_recursive_mutex));
if (mtx == NULL)
return NULL;
chMtxInit(&mtx->mtx);
mtx->count = 0;
return mtx;
}
/**
* @brief Allocate a new device
*/
static struct bma180_dev * PIOS_BMA180_alloc(void)
{
struct bma180_dev * bma180_dev;
bma180_dev = (struct bma180_dev *)PIOS_malloc(sizeof(*bma180_dev));
if (!bma180_dev) return (NULL);
fifoBuf_init(&bma180_dev->fifo, (uint8_t *) bma180_dev->buffer, sizeof(bma180_dev->buffer));
bma180_dev->magic = PIOS_BMA180_DEV_MAGIC;
return(bma180_dev);
}
static struct pios_can_dev *PIOS_CAN_alloc(void)
{
struct pios_can_dev *can_dev;
can_dev = (struct pios_can_dev *)PIOS_malloc(sizeof(*can_dev));
if (!can_dev) return(NULL);
memset(can_dev, 0, sizeof(*can_dev));
can_dev->magic = PIOS_CAN_DEV_MAGIC;
return(can_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;
}
/**
* 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;
}
/**
* ChibiOS stack expects alignment (both start and end)
* to 8 byte boundaries. This makes sure to allocate enough
* memory and return an address that has the requested size
* or more with these constraints.
*/
static uint8_t * align8_alloc(uint32_t size)
{
// round size up to at nearest multiple of 8 + 4 bytes to guarantee
// sufficient size within. This is because PIOS_malloc only guarantees
// uintptr_t alignment which is 4 bytes.
size = size + sizeof(uintptr_t);
uint8_t *wap = PIOS_malloc(size);
// shift start point to nearest 8 byte boundary.
uint32_t pad = ((uint32_t) wap) % sizeof(stkalign_t);
wap = wap + pad;
return wap;
}
/**
* @brief Allocates an internal ADC device in memory
* \param[out] pointer to the newly created device
*
*/
static struct pios_internal_adc_dev * PIOS_INTERNAL_ADC_Allocate()
{
if(static_adc_dev)
return NULL;
struct pios_internal_adc_dev * adc_dev;
adc_dev = (struct pios_internal_adc_dev *)PIOS_malloc(sizeof(*adc_dev));
if (!adc_dev) return (NULL);
adc_dev->magic = PIOS_INTERNAL_ADC_DEV_MAGIC;
static_adc_dev = adc_dev;
return(adc_dev);
}
int sin_lookup_initialize()
{
// Previously initialized
if (sin_table)
return 0;
sin_table = (float *) PIOS_malloc(sizeof(float) * SIN_RESOLUTION);
if (sin_table == NULL)
return -1;
for(uint32_t i = 0; i < 180; i++)
sin_table[i] = sinf((float)i * DEG2RAD);
return 0;
}
static struct pios_gcsrcvr_dev *PIOS_gcsrcvr_alloc(void)
{
struct pios_gcsrcvr_dev * gcsrcvr_dev;
gcsrcvr_dev = (struct pios_gcsrcvr_dev *)PIOS_malloc(sizeof(*gcsrcvr_dev));
if (!gcsrcvr_dev) return(NULL);
gcsrcvr_dev->magic = PIOS_GCSRCVR_DEV_MAGIC;
gcsrcvr_dev->Fresh = false;
gcsrcvr_dev->supv_timer = 0;
/* The update callback cannot receive the device pointer, so set it in a global */
global_gcsrcvr_dev = gcsrcvr_dev;
return(gcsrcvr_dev);
}
static struct pios_rfm22b_rcvr_dev *PIOS_RFM22B_Rcvr_alloc(void)
{
struct pios_rfm22b_rcvr_dev *rfm22b_rcvr_dev;
rfm22b_rcvr_dev =
(struct pios_rfm22b_rcvr_dev *)
PIOS_malloc(sizeof(*rfm22b_rcvr_dev));
if (!rfm22b_rcvr_dev) {
return NULL;
}
rfm22b_rcvr_dev->magic = PIOS_RFM22B_RCVR_DEV_MAGIC;
rfm22b_rcvr_dev->fresh = false;
rfm22b_rcvr_dev->supv_timer = 0;
return rfm22b_rcvr_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 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;
}