/**
* @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;
}
/**
* Initialise the telemetry module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t TelemetryInitialize(void)
{
FlightTelemetryStatsInitialize();
GCSTelemetryStatsInitialize();
// Initialize vars
timeOfLastObjectUpdate = 0;
// Create object queues
queue = PIOS_Queue_Create(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
priorityQueue = PIOS_Queue_Create(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
#endif
// Update telemetry settings
updateSettings();
// Initialise UAVTalk
uavTalkCon = UAVTalkInitialize(&transmitData);
// Create periodic event that will be used to update the telemetry stats
txErrors = 0;
txRetries = 0;
UAVObjEvent ev;
memset(&ev, 0, sizeof(UAVObjEvent));
EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS);
SessionManagingInitialize();
SessionManagingConnectCallback(session_managing_updated);
//register the new uavo instance callback function in the uavobjectmanager
UAVObjRegisterNewInstanceCB(update_object_instances);
return 0;
}
/**
* @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;
}
/**
* Module initialization
*/
int32_t PathPlannerInitialize()
{
taskHandle = NULL;
#ifdef MODULE_PathPlanner_BUILTIN
module_enabled = true;
#else
uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_state);
if (module_state[MODULESETTINGS_ADMINSTATE_PATHPLANNER] == MODULESETTINGS_ADMINSTATE_ENABLED) {
module_enabled = true;
} else {
module_enabled = false;
}
#endif
if(module_enabled) {
PathPlannerSettingsInitialize();
WaypointInitialize();
WaypointActiveInitialize();
// Create object queue
queue = PIOS_Queue_Create(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
return 0;
}
return -1;
}
/**
* @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, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AltitudeHoldInitialize()
{
#ifdef MODULE_AltitudeHold_BUILTIN
module_enabled = true;
#else
uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_state);
if (module_state[MODULESETTINGS_ADMINSTATE_ALTITUDEHOLD] == MODULESETTINGS_ADMINSTATE_ENABLED) {
module_enabled = true;
} else {
module_enabled = false;
}
#endif
if(module_enabled) {
AltitudeHoldSettingsInitialize();
AltitudeHoldDesiredInitialize();
// Create object queue
queue = PIOS_Queue_Create(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
return 0;
}
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;
}
// 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;
}
/**
* Initialise the UAVORelay module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t UAVORelayInitialize(void)
{
// TODO: make selectable
uavorelay_com_id = pios_com_can_id;
#ifdef MODULE_UAVORelay_BUILTIN
module_enabled = true;
#else
uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_state);
if (module_state[MODULESETTINGS_ADMINSTATE_UAVORELAY] == MODULESETTINGS_ADMINSTATE_ENABLED) {
module_enabled = true;
} else {
module_enabled = false;
}
#endif
if (!uavorelay_com_id)
module_enabled = false;
if (!module_enabled)
return -1;
// Create object queues
queue = PIOS_Queue_Create(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Initialise UAVTalk
uavTalkCon = UAVTalkInitialize(&send_data);
CameraDesiredInitialize();
return 0;
}
/**
* Create a queue to receive messages for a particular message
* and return it
* @param[in] id the CAN device ID
* @param[in] msg_id The message ID (std ID < 0x7FF)
*/
struct pios_queue * PIOS_CAN_RegisterMessageQueue(uintptr_t id, enum pios_can_messages msg_id)
{
// Fetch the size of this message type or error if unknown
uint32_t bytes;
switch(msg_id) {
case PIOS_CAN_GIMBAL:
bytes = sizeof(struct pios_can_gimbal_message);
break;
default:
return NULL;
}
// Return existing queue if created
if (pios_can_queues[msg_id] != NULL)
return pios_can_queues[msg_id];
// Create a queue that can manage the data message size
struct pios_queue *queue;
queue = PIOS_Queue_Create(2, bytes);
if (queue == NULL)
return NULL;
// Store the queue handle for the driver
pios_can_queues[msg_id] = queue;
return queue;
}
/**
* Initialise the overo sync module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t OveroSyncInitialize(void)
{
#ifdef MODULE_OveroSync_BUILTIN
module_enabled = true;
#else
uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_state);
if (module_state[MODULESETTINGS_ADMINSTATE_OVEROSYNC] == MODULESETTINGS_ADMINSTATE_ENABLED) {
module_enabled = true;
} else {
module_enabled = false;
}
#endif
if (!module_enabled)
return -1;
// Create object queues
queue = PIOS_Queue_Create(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
OveroSyncStatsInitialize();
// Initialise UAVTalk
uavTalkCon = UAVTalkInitialize(&pack_data);
return 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;
}
/**
* @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 Module initialization
* @return 0
*/
static int32_t BrushlessGimbalInitialize()
{
// Listen for ActuatorDesired updates (Primary input to this module)
ActuatorDesiredInitialize();
queue = PIOS_Queue_Create(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
ActuatorDesiredConnectQueue(queue);
BrushlessGimbalSettingsInitialize();
return 0;
}
/**
* Initialise the telemetry module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t OveroSyncInitialize(void)
{
OveroSyncStatsInitialize();
// Create object queues
queue = PIOS_Queue_Create(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Initialise UAVTalk
uavTalkCon = UAVTalkInitialize(&packData);
return 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;
}
/**
* 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;
}
/**
* Initialize the module, called on startup.
* \returns 0 on success or -1 if initialization failed
*/
int32_t SystemModInitialize(void)
{
// Must registers objects here for system thread because ObjectManager started in OpenPilotInit
SystemSettingsInitialize();
SystemStatsInitialize();
FlightStatusInitialize();
ObjectPersistenceInitialize();
#if defined(DIAG_TASKS)
TaskInfoInitialize();
#endif
#if defined(WDG_STATS_DIAGNOSTICS)
WatchdogStatusInitialize();
#endif
objectPersistenceQueue = PIOS_Queue_Create(1, sizeof(UAVObjEvent));
if (objectPersistenceQueue == NULL)
return -1;
SystemModStart();
return 0;
}
