Syslink::Syslink() :
pktrate(0),
nullrate(0),
rxrate(0),
txrate(0),
_syslink_task(-1),
_task_running(false),
_bootloader_mode(false),
_count(0),
_null_count(0),
_count_in(0),
_count_out(0),
_lasttime(0),
_lasttxtime(0),
_lastrxtime(0),
_fd(0),
_queue(2, sizeof(syslink_message_t)),
_writebuffer(16, sizeof(crtp_message_t)),
_battery_pub(nullptr),
_rc_pub(nullptr),
_cmd_pub(nullptr),
_rssi(RC_INPUT_RSSI_MAX),
_bstate(BAT_DISCHARGING)
{
px4_sem_init(&memory_sem, 0, 0);
/* memory_sem use case is a signal */
px4_sem_setprotocol(&memory_sem, SEM_PRIO_NONE);
}
int test_dataman(int argc, char *argv[])
{
int i, 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));
warnx("Running %d tasks", num_tasks);
for (i = 0; i < num_tasks; i++) {
int task;
char a[16];
sprintf(a, "%d", i);
const char *av[2];
av[0] = a;
av[1] = 0;
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, (void *)av)) <= 0) {
warn("task start failed");
}
}
for (i = 0; i < num_tasks; i++) {
px4_sem_wait(sems + i);
px4_sem_destroy(sems + i);
}
free(sems);
dm_restart(DM_INIT_REASON_IN_FLIGHT);
for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
break;
}
}
if (i >= NUM_MISSIONS_SUPPORTED) {
warnx("Restart in-flight failed");
return -1;
}
dm_restart(DM_INIT_REASON_POWER_ON);
for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
warnx("Restart power-on failed");
return -1;
}
}
return 0;
}
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
CDev("uavcan", UAVCAN_DEVICE_PATH),
_node(can_driver, system_clock, _pool_allocator),
_node_mutex(),
_esc_controller(_node),
_hardpoint_controller(_node),
_time_sync_master(_node),
_time_sync_slave(_node),
_node_status_monitor(_node),
_master_timer(_node),
_setget_response(0)
{
_task_should_exit = false;
_fw_server_action = None;
_fw_server_status = -1;
_tx_injector = nullptr;
_control_topics[0] = ORB_ID(actuator_controls_0);
_control_topics[1] = ORB_ID(actuator_controls_1);
_control_topics[2] = ORB_ID(actuator_controls_2);
_control_topics[3] = ORB_ID(actuator_controls_3);
int res = pthread_mutex_init(&_node_mutex, nullptr);
if (res < 0) {
std::abort();
}
res = px4_sem_init(&_server_command_sem, 0 , 0);
if (res < 0) {
std::abort();
}
/* _server_command_sem use case is a signal */
px4_sem_setprotocol(&_server_command_sem, SEM_PRIO_NONE);
if (_perfcnt_node_spin_elapsed == nullptr) {
errx(1, "uavcan: couldn't allocate _perfcnt_node_spin_elapsed");
}
if (_perfcnt_esc_mixer_output_elapsed == nullptr) {
errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_output_elapsed");
}
if (_perfcnt_esc_mixer_total_elapsed == nullptr) {
errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_total_elapsed");
}
}
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;
}
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;
}
