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);
}
