UavcanNode::~UavcanNode() { fw_server(Stop); if (_task != -1) { /* tell the task we want it to go away */ _task_should_exit = true; unsigned i = 10; do { /* wait 5ms - it should wake every 10ms or so worst-case */ usleep(5000); /* if we have given up, kill it */ if (--i == 0) { task_delete(_task); break; } } while (_task != -1); } (void)orb_unsubscribe(_armed_sub); (void)orb_unsubscribe(_test_motor_sub); (void)orb_unsubscribe(_actuator_direct_sub); // Removing the sensor bridges auto br = _sensor_bridges.getHead(); while (br != nullptr) { auto next = br->getSibling(); delete br; br = next; } _instance = nullptr; perf_free(_perfcnt_node_spin_elapsed); perf_free(_perfcnt_esc_mixer_output_elapsed); perf_free(_perfcnt_esc_mixer_total_elapsed); pthread_mutex_destroy(&_node_mutex); px4_sem_destroy(&_server_command_sem); // Is it allowed to delete it like that? if (_mixers != nullptr) { delete _mixers; } }
static inline void destroy_work_item(work_q_item_t *item) { px4_sem_destroy(&item->wait_sem); /* Destroy the item lock */ /* Return the item to the free item queue for later reuse */ lock_queue(&g_free_q); sq_addfirst(&item->link, &(g_free_q.q)); /* Update the queue size and potentially the maximum queue size */ if (++g_free_q.size > g_free_q.max_size) { g_free_q.max_size = g_free_q.size; } unlock_queue(&g_free_q); }
static int start(void) { int task; px4_sem_init(&g_init_sema, 1, 0); /* start the worker thread with low priority for disk IO */ if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 10, 1200, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } /* wait for the thread to actually initialize */ px4_sem_wait(&g_init_sema); px4_sem_destroy(&g_init_sema); return 0; }
Device::~Device() { px4_sem_destroy(&_lock); }
static int task_main(int argc, char *argv[]) { work_q_item_t *work; /* Initialize global variables */ g_key_offsets[0] = 0; for (unsigned i = 0; i < (DM_KEY_NUM_KEYS - 1); i++) { g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * k_sector_size); } unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * k_sector_size); for (unsigned i = 0; i < dm_number_of_funcs; i++) { g_func_counts[i] = 0; } /* Initialize the item type locks, for now only DM_KEY_MISSION_STATE supports locking */ px4_sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */ for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) { g_item_locks[i] = NULL; } g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex; g_task_should_exit = false; init_q(&g_work_q); init_q(&g_free_q); px4_sem_init(&g_work_queued_sema, 1, 0); /* See if the data manage file exists and is a multiple of the sector size */ g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY); if (g_task_fd >= 0) { #ifndef __PX4_POSIX // XXX on Mac OS and Linux the file is not a multiple of the sector sizes // this might need further inspection. /* File exists, check its size */ int file_size = lseek(g_task_fd, 0, SEEK_END); if ((file_size % k_sector_size) != 0) { PX4_WARN("Incompatible data manager file %s, resetting it", k_data_manager_device_path); PX4_WARN("Size: %u, sector size: %d", file_size, k_sector_size); close(g_task_fd); unlink(k_data_manager_device_path); } #else close(g_task_fd); #endif } /* Open or create the data manager file */ g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, PX4_O_MODE_666); if (g_task_fd < 0) { PX4_WARN("Could not open data manager file %s", k_data_manager_device_path); px4_sem_post(&g_init_sema); /* Don't want to hang startup */ return -1; } if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) { close(g_task_fd); PX4_WARN("Could not seek data manager file %s", k_data_manager_device_path); px4_sem_post(&g_init_sema); /* Don't want to hang startup */ return -1; } fsync(g_task_fd); /* see if we need to erase any items based on restart type */ int sys_restart_val; const char *restart_type_str = "Unkown restart"; if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) { if (sys_restart_val == DM_INIT_REASON_POWER_ON) { restart_type_str = "Power on restart"; _restart(DM_INIT_REASON_POWER_ON); } else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { restart_type_str = "In flight restart"; _restart(DM_INIT_REASON_IN_FLIGHT); } } /* We use two file descriptors, one for the caller context and one for the worker thread */ /* They are actually the same but we need to some way to reject caller request while the */ /* worker thread is shutting down but still processing requests */ g_fd = g_task_fd; PX4_INFO("%s, data manager file '%s' size is %d bytes", restart_type_str, k_data_manager_device_path, max_offset); /* Tell startup that the worker thread has completed its initialization */ px4_sem_post(&g_init_sema); /* Start the endless loop, waiting for then processing work requests */ while (true) { /* do we need to exit ??? */ if ((g_task_should_exit) && (g_fd >= 0)) { /* Close the file handle to stop further queuing */ g_fd = -1; } if (!g_task_should_exit) { /* wait for work */ px4_sem_wait(&g_work_queued_sema); } /* Empty the work queue */ while ((work = dequeue_work_item())) { /* handle each work item with the appropriate handler */ switch (work->func) { case dm_write_func: g_func_counts[dm_write_func]++; work->result = _write(work->write_params.item, work->write_params.index, work->write_params.persistence, work->write_params.buf, work->write_params.count); break; case dm_read_func: g_func_counts[dm_read_func]++; work->result = _read(work->read_params.item, work->read_params.index, work->read_params.buf, work->read_params.count); break; case dm_clear_func: g_func_counts[dm_clear_func]++; work->result = _clear(work->clear_params.item); break; case dm_restart_func: g_func_counts[dm_restart_func]++; work->result = _restart(work->restart_params.reason); break; default: /* should never happen */ work->result = -1; break; } /* Inform the caller that work is done */ px4_sem_post(&work->wait_sem); } /* time to go???? */ if ((g_task_should_exit) && (g_fd < 0)) { break; } } close(g_task_fd); g_task_fd = -1; /* The work queue is now empty, empty the free queue */ for (;;) { if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL) { break; } if (work->first) { free(work); } } destroy_q(&g_work_q); destroy_q(&g_free_q); px4_sem_destroy(&g_work_queued_sema); px4_sem_destroy(&g_sys_state_mutex); return 0; }
static inline void destroy_q(work_q_t *q) { px4_sem_destroy(&(q->mutex)); /* Destroy the queue lock */ }
int test_dataman(int argc, char *argv[]) { int i = 0; unsigned num_tasks = 4; char buffer[DM_MAX_DATA_SIZE]; if (argc > 1) { num_tasks = atoi(argv[1]); } sems = (px4_sem_t *)malloc(num_tasks * sizeof(px4_sem_t)); task_returned_error = (bool *)calloc(num_tasks, sizeof(bool)); PX4_INFO("Running %d tasks", num_tasks); for (i = 0; i < num_tasks; i++) { int task; char a[16]; snprintf(a, 16, "%d", i); char *av[] = {"tests_dataman", a, NULL}; px4_sem_init(sems + i, 1, 0); /* sems use case is a signal */ px4_sem_setprotocol(sems, SEM_PRIO_NONE); /* start the task */ if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, av)) <= 0) { PX4_ERR("task start failed"); } } for (i = 0; i < num_tasks; i++) { px4_sem_wait(sems + i); px4_sem_destroy(sems + i); } free(sems); bool got_error = false; for (i = 0; i < num_tasks; i++) { if (task_returned_error[i]) { got_error = true; break; } } free(task_returned_error); if (got_error) { return -1; } dm_restart(DM_INIT_REASON_IN_FLIGHT); for (i = 0; i < NUM_MISSIONS_TEST; i++) { if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) { break; } } if (i >= NUM_MISSIONS_TEST) { PX4_ERR("Restart in-flight failed"); return -1; } dm_restart(DM_INIT_REASON_POWER_ON); for (i = 0; i < NUM_MISSIONS_TEST; i++) { if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) { PX4_ERR("Restart power-on failed"); return -1; } } return 0; }
uORB::DeviceMaster::~DeviceMaster() { px4_sem_destroy(&_lock); }
PX4IO_serial_f4::PX4IO_serial_f4() : _tx_dma(nullptr), _rx_dma(nullptr), _current_packet(nullptr), _rx_dma_status(_dma_status_inactive), _completion_semaphore(SEM_INITIALIZER(0)), #if 0 _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")), _pc_dmaerrs(perf_alloc(PC_COUNT, "io_dmaerrs ")) #else _pc_dmasetup(nullptr), _pc_dmaerrs(nullptr) #endif { } PX4IO_serial_f4::~PX4IO_serial_f4() { if (_tx_dma != nullptr) { stm32_dmastop(_tx_dma); stm32_dmafree(_tx_dma); } if (_rx_dma != nullptr) { stm32_dmastop(_rx_dma); stm32_dmafree(_rx_dma); } /* reset the UART */ rCR1 = 0; rCR2 = 0; rCR3 = 0; /* detach our interrupt handler */ up_disable_irq(PX4IO_SERIAL_VECTOR); irq_detach(PX4IO_SERIAL_VECTOR); /* restore the GPIOs */ px4_arch_unconfiggpio(PX4IO_SERIAL_TX_GPIO); px4_arch_unconfiggpio(PX4IO_SERIAL_RX_GPIO); /* Disable APB clock for the USART peripheral */ modifyreg32(PX4IO_SERIAL_RCC_REG, PX4IO_SERIAL_RCC_EN, 0); /* and kill our semaphores */ px4_sem_destroy(&_completion_semaphore); perf_free(_pc_dmasetup); perf_free(_pc_dmaerrs); } int PX4IO_serial_f4::init() { /* initialize base implementation */ int r; if ((r = PX4IO_serial::init(&_io_buffer_storage)) != 0) { return r; } /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); _rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP); if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) { return -1; } /* Enable the APB clock for the USART peripheral */ modifyreg32(PX4IO_SERIAL_RCC_REG, 0, PX4IO_SERIAL_RCC_EN); /* configure pins for serial use */ px4_arch_configgpio(PX4IO_SERIAL_TX_GPIO); px4_arch_configgpio(PX4IO_SERIAL_RX_GPIO); /* reset & configure the UART */ rCR1 = 0; rCR2 = 0; rCR3 = 0; /* eat any existing interrupt status */ (void)rSR; (void)rDR; /* configure line speed */ uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2); uint32_t mantissa = usartdiv32 >> 5; uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); /* attach serial interrupt handler */ irq_attach(PX4IO_SERIAL_VECTOR, _interrupt, this); up_enable_irq(PX4IO_SERIAL_VECTOR); /* enable UART in DMA mode, enable error and line idle interrupts */ rCR3 = USART_CR3_EIE; rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; /* create semaphores */ px4_sem_init(&_completion_semaphore, 0, 0); /* _completion_semaphore use case is a signal */ px4_sem_setprotocol(&_completion_semaphore, SEM_PRIO_NONE); /* XXX this could try talking to IO */ return 0; }
PX4IO_serial_f7::PX4IO_serial_f7() : _tx_dma(nullptr), _rx_dma(nullptr), _current_packet(nullptr), _rx_dma_status(_dma_status_inactive), _completion_semaphore(SEM_INITIALIZER(0)), #if 0 _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")), _pc_dmaerrs(perf_alloc(PC_COUNT, "io_dmaerrs ")) #else _pc_dmasetup(nullptr), _pc_dmaerrs(nullptr) #endif { } PX4IO_serial_f7::~PX4IO_serial_f7() { if (_tx_dma != nullptr) { stm32_dmastop(_tx_dma); stm32_dmafree(_tx_dma); } if (_rx_dma != nullptr) { stm32_dmastop(_rx_dma); stm32_dmafree(_rx_dma); } /* reset the UART */ rCR1 = 0; rCR2 = 0; rCR3 = 0; /* detach our interrupt handler */ up_disable_irq(PX4IO_SERIAL_VECTOR); irq_detach(PX4IO_SERIAL_VECTOR); /* restore the GPIOs */ px4_arch_unconfiggpio(PX4IO_SERIAL_TX_GPIO); px4_arch_unconfiggpio(PX4IO_SERIAL_RX_GPIO); /* Disable APB clock for the USART peripheral */ modifyreg32(PX4IO_SERIAL_RCC_REG, PX4IO_SERIAL_RCC_EN, 0); /* and kill our semaphores */ px4_sem_destroy(&_completion_semaphore); perf_free(_pc_dmasetup); perf_free(_pc_dmaerrs); } int PX4IO_serial_f7::init() { /* initialize base implementation */ int r; if ((r = PX4IO_serial::init((IOPacket *)ROUND_UP_TO_POW2_CT((uintptr_t)_io_buffer_storage, CACHE_LINE_SIZE))) != 0) { return r; } /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); _rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP); if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) { return -1; } /* Enable the APB clock for the USART peripheral */ modifyreg32(PX4IO_SERIAL_RCC_REG, 0, PX4IO_SERIAL_RCC_EN); /* configure pins for serial use */ px4_arch_configgpio(PX4IO_SERIAL_TX_GPIO); px4_arch_configgpio(PX4IO_SERIAL_RX_GPIO); /* reset & configure the UART */ rCR1 = 0; rCR2 = 0; rCR3 = 0; /* clear data that may be in the RDR and clear overrun error: */ if (rISR & USART_ISR_RXNE) { (void)rRDR; } rICR = rISR & rISR_ERR_FLAGS_MASK; /* clear the flags */ /* configure line speed */ uint32_t usartdiv32 = (PX4IO_SERIAL_CLOCK + (PX4IO_SERIAL_BITRATE) / 2) / (PX4IO_SERIAL_BITRATE); rBRR = usartdiv32; /* attach serial interrupt handler */ irq_attach(PX4IO_SERIAL_VECTOR, _interrupt, this); up_enable_irq(PX4IO_SERIAL_VECTOR); /* enable UART in DMA mode, enable error and line idle interrupts */ rCR3 = USART_CR3_EIE; /* TODO: maybe use DDRE */ rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; /* TODO: maybe we need to adhere to the procedure as described in the reference manual page 1251 (34.5.2) */ /* create semaphores */ px4_sem_init(&_completion_semaphore, 0, 0); /* _completion_semaphore use case is a signal */ px4_sem_setprotocol(&_completion_semaphore, SEM_PRIO_NONE); /* XXX this could try talking to IO */ return 0; }
MavlinkCommandSender::~MavlinkCommandSender() { px4_sem_destroy(&_lock); }