void task_main(int argc, char *argv[])
{
PX4_WARN("enter mpu9x50 task_main");
sigset_t set;
int sig = 0;
int rv;
exit_mreasurement = false;
parameters_init();
// create the mpu9x50 default configuration structure
struct mpu9x50_config config = {
.gyro_lpf = _gyro_lpf,
.acc_lpf = _accel_lpf,
.gyro_fsr = MPU9X50_GYRO_FSR_500DPS,
.acc_fsr = MPU9X50_ACC_FSR_4G,
.gyro_sample_rate = _imu_sample_rate,
.compass_enabled = true,
.compass_sample_rate = MPU9x50_COMPASS_SAMPLE_RATE_100HZ,
.spi_dev_path = _device,
};
// TODO-JYW:
// manually register with the DriverFramework to allow this driver to
// be found by other modules
// DriverFramework::StubSensor<DriverFramework::SPIDevObj> stub_sensor("mpu9x50", "/dev/accel0", "/dev/accel");
if (mpu9x50_initialize(&config) != 0) {
PX4_WARN("error initializing mpu9x50. Quit!");
goto exit;
}
if (mpu9x50_register_interrupt(SPI_INT_GPIO, &mpu9x50::data_ready_isr, NULL)
//if (mpu9x50_register_interrupt(SPI_INT_GPIO, &mpu9x50::data_ready_isr, NULL)
!= 0) {
PX4_WARN("error registering data ready interrupt. Quit!");
goto exit;
}
// create all uorb publications
if (!create_pubs()) {
goto exit;
}
// subscribe to parameter_update topic
_params_sub = orb_subscribe(ORB_ID(parameter_update));
// initialize signal
sigemptyset(&set);
sigaddset(&set, SIGRTMIN);
while (!_task_should_exit) {
// wait on signal
rv = sigwait(&set, &sig);
// check if we are waken up by the proper signal
if (rv != 0 || sig != SIGRTMIN) {
PX4_WARN("mpu9x50 sigwait failed rv %d sig %d", rv, sig);
continue;
}
// read new IMU data and store the results in data
if (mpu9x50_get_data(&_data) != 0) {
PX4_WARN("mpu9x50_get_data() failed");
continue;
}
// since the context switch takes long time, override the timestamp provided by mpu9x50_get_data()
// with the ts of isr.
// Note: This is ok for MPU sample rate of < 1000; Higer than 1000 Sample rates will be an issue
// as the data is not consistent.
_data.timestamp = _isr_data_ready_timestamp;
// poll parameter update
parameter_update_poll();
// data is ready
update_reports();
// publish all sensor reports
publish_reports();
}
exit_mreasurement = true;
exit:
PX4_WARN("closing mpu9x50");
mpu9x50_close();
}
/** mpu9x50 main entrance */
void task_main_trampoline(int argc, char *argv[])
{
PX4_WARN("task_main_trampoline");
task_main(argc, argv);
}
void task_main_trampoline(int argc, char *argv[])
{
task_main(argc, argv);
}
/** uart_esc main entrance */
void task_main_trampoline(int argc, char *argv[])
{
PX4_WARN("task_main_trampoline");
task_main(argc, argv);
}
