/** * @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; }