42 #include <px4_arch/px4io_serial.h> 49 return new ArchPX4IOSerial();
52 PX4IO_serial::PX4IO_serial() :
53 Device(
"PX4IO_serial"),
65 _pc_timeouts(
nullptr),
67 _pc_protoerrs(
nullptr),
72 _bus_semaphore(SEM_INITIALIZER(0))
77 PX4IO_serial::~PX4IO_serial()
80 px4_sem_destroy(&_bus_semaphore);
99 _io_buffer_ptr = io_buffer;
102 px4_sem_init(&_bus_semaphore, 0, 1);
110 uint8_t page = address >> 8;
111 uint8_t offset = address & 0xff;
112 const uint16_t *values =
reinterpret_cast<const uint16_t *
>(
data);
118 px4_sem_wait(&_bus_semaphore);
122 for (
unsigned retries = 0; retries < 3; retries++) {
124 _io_buffer_ptr->page = page;
125 _io_buffer_ptr->offset = offset;
126 memcpy((
void *)&_io_buffer_ptr->regs[0], (
void *)values, (2 * count));
129 _io_buffer_ptr->regs[i] = 0x55aa;
132 _io_buffer_ptr->crc = 0;
133 _io_buffer_ptr->crc =
crc_packet(_io_buffer_ptr);
136 result = _bus_exchange(_io_buffer_ptr);
155 px4_sem_post(&_bus_semaphore);
167 uint8_t page = address >> 8;
168 uint8_t offset = address & 0xff;
169 uint16_t *values =
reinterpret_cast<uint16_t *
>(
data);
175 px4_sem_wait(&_bus_semaphore);
179 for (
unsigned retries = 0; retries < 3; retries++) {
182 _io_buffer_ptr->page = page;
183 _io_buffer_ptr->offset = offset;
185 _io_buffer_ptr->crc = 0;
186 _io_buffer_ptr->crc =
crc_packet(_io_buffer_ptr);
189 result = _bus_exchange(_io_buffer_ptr);
203 }
else if (
PKT_COUNT(*_io_buffer_ptr) != count) {
214 memcpy(values, &_io_buffer_ptr->regs[0], (2 * count));
223 px4_sem_post(&_bus_semaphore);
measure the time elapsed performing an event
count the number of times an event occurs
static void read(bootloader_app_shared_t *pshared)
void perf_count(perf_counter_t handle)
Count a performance event.
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused))
#define PKT_MAX_REGS
Serial protocol encapsulation.
static void write(bootloader_app_shared_t *pshared)
static PX4IO_serial * g_interface
Fundamental base class for all physical drivers (I2C, SPI).
device::Device * PX4IO_serial_interface()