I am writing an external IMU driver for the PX4 Cube that communicates via UART. Everything works smoothly until I try and access the UART read() function (multiple times per iteration to read the entire buffer) from inside a hal.scheduler timer process, at which point the PX4 completely dies. I have attempted this with uartC and uartE and get the same result each time. I can access the UART just fine from within the AP_InertialSensor::update() function, but unfortunately ArduPilot is set up so that I need to call _notify_new_gyro_raw_sample/_notify_new_accel_raw_sample from within the timer process for the code to work. Has anyone run into this issue before, and if so, how did you work around it?
It’s a VectorNav VN-100/VN-110, but honestly the only relevant information is that I can’t access the UART within a timer loop. The problem persists if I try and do this with any UART device.
I figured it out. Turns out the UARTDriver read() function has a catch (returns -1/0xFF) to restrict it to the thread in which its object was instantiated. This prevented it from working within any of the timer threads I tried and lead to problems that would crash the entire program.
hi William,
as you mentioned above,i removed return -1 as below,is it right?
int16_t UARTDriver::read()
{
if (lock_read_key != 0 || _uart_owner_thd != chThdGetSelfX()){ //return -1;
}
if (!_initialised) {
return -1;
}
uint8_t byte;
if (!_readbuf.read_byte(&byte)) {
return -1;
}
if (!_rts_is_active) {
update_rts_line();
}
return byte;
Ok. Actually I am very new to this. Can you guide me or any reference how to write the driver. As i have seen the ardupilot site where driver article is there. But it looks complex. Can you guide me for the same?