ardupilot如何读取传感器数据

错误挺多，还未改正，请见谅。

1.setup();loop();

由HAL_PX4_Class.cpp可知，g_callbacks->setup();运行一次，g_callbacks->loop();在循环里

static int main_loop(int argc, char **argv){    hal.uartA->begin(115200);    hal.uartB->begin(38400);    hal.uartC->begin(57600);    hal.uartD->begin(57600);    hal.uartE->begin(57600);    hal.scheduler->init();    /*      run setup() at low priority to ensure CLI doesn't hang the      system, and to allow initial sensor read loops to run     */    hal_px4_set_priority(APM_STARTUP_PRIORITY);    schedulerInstance.hal_initialized();    g_callbacks->setup();    hal.scheduler->system_initialized();    perf_counter_t perf_loop = perf_alloc(PC_ELAPSED, "APM_loop");    perf_counter_t perf_overrun = perf_alloc(PC_COUNT, "APM_overrun");    struct hrt_call loop_overtime_call;    thread_running = true;    /*      switch to high priority for main loop     */    hal_px4_set_priority(APM_MAIN_PRIORITY);    while (!_px4_thread_should_exit) {        perf_begin(perf_loop);        /*          this ensures a tight loop waiting on a lower priority driver          will eventually give up some time for the driver to run. It          will only ever be called if a loop() call runs for more than          0.1 second         */        hrt_call_after(&loop_overtime_call, 100000, (hrt_callout)loop_overtime, nullptr);        g_callbacks->loop();        if (px4_ran_overtime) {            /*              we ran over 1s in loop(), and our priority was lowered              to let a driver run. Set it back to high priority now.             */            hal_px4_set_priority(APM_MAIN_PRIORITY);            perf_count(perf_overrun);            px4_ran_overtime = false;        }        perf_end(perf_loop);        /*          give up 250 microseconds of time, to ensure drivers get a          chance to run. This relies on the accurate semaphore wait          using hrt in semaphore.cpp         */        hal.scheduler->delay_microseconds(250);    }    thread_running = false;    return 0;}

对应到ArduCoptor.cpp就是Copter::setup()运行一次(也就是初始化)，Copter::loop()循环运行(程序主体)。

2.init_ardupilot()注册传感器

请注意初始化里的init_ardupilot();

void Copter::setup() {    cliSerial = hal.console;    // Load the default values of variables listed in var_info[]s    AP_Param::setup_sketch_defaults();    // setup storage layout for copter    StorageManager::set_layout_copter();    init_ardupilot();    // initialise the main loop scheduler    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));    // setup initial performance counters    perf_info_reset();    fast_loopTimer = AP_HAL::micros();}

请注意BoardConfig.init();

void Copter::init_ardupilot(){    if (!hal.gpio->usb_connected()) {        // USB is not connected, this means UART0 may be a Xbee, with        // its darned bricking problem. We can't write to it for at        // least one second after powering up. Simplest solution for        // now is to delay for 1 second. Something more elegant may be        // added later        delay(1000);    }    // initialise serial port    serial_manager.init_console();    // init vehicle capabilties    init_capabilities();    cliSerial->printf("\n\nInit " FIRMWARE_STRING                         "\n\nFree RAM: %u\n",                      (unsigned)hal.util->available_memory());    //    // Report firmware version code expect on console (check of actual EEPROM format version is done in load_parameters function)    //    report_version();    // load parameters from EEPROM    load_parameters();    // initialise stats module    g2.stats.init();    GCS_MAVLINK::set_dataflash(&DataFlash);    // identify ourselves correctly with the ground station    mavlink_system.sysid = g.sysid_this_mav;    // initialise serial ports    serial_manager.init();    // setup first port early to allow BoardConfig to report errors    gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);    // Register mavlink_delay_cb, which will run anytime you have    // more than 5ms remaining in your call to hal.scheduler->delay    hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);    BoardConfig.init();    // init cargo gripper#if GRIPPER_ENABLED == ENABLED    g2.gripper.init();#endif    // initialise notify system    // disable external leds if epm is enabled because of pin conflict on the APM    notify.init(true);    // initialise battery monitor    battery.init();    // Init RSSI    rssi.init();    barometer.init();    // we start by assuming USB connected, as we initialed the serial    // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.    ap.usb_connected = true;    check_usb_mux();    // setup telem slots with serial ports    for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) {        gcs[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i);    }#if FRSKY_TELEM_ENABLED == ENABLED    // setup frsky, and pass a number of parameters to the library    frsky_telemetry.init(serial_manager, FIRMWARE_STRING " " FRAME_CONFIG_STRING,                         FRAME_MAV_TYPE,                         &g.fs_batt_voltage, &g.fs_batt_mah, &ap.value);#endif#if LOGGING_ENABLED == ENABLED    log_init();#endif    // update motor interlock state    update_using_interlock();#if FRAME_CONFIG == HELI_FRAME    // trad heli specific initialisation    heli_init();#endif    init_rc_in();               // sets up rc channels from radio    init_rc_out();              // sets up motors and output to escs    // initialise which outputs Servo and Relay events can use    ServoRelayEvents.set_channel_mask(~motors.get_motor_mask());    relay.init();    /*     *  setup the 'main loop is dead' check. Note that this relies on     *  the RC library being initialised.     */    hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);    // give AHRS the rnage beacon sensor    ahrs.set_beacon(&g2.beacon);    // Do GPS init    gps.init(&DataFlash, serial_manager);    if(g.compass_enabled)        init_compass();#if OPTFLOW == ENABLED    // make optflow available to AHRS    ahrs.set_optflow(&optflow);#endif    // init Location class    Location_Class::set_ahrs(&ahrs);#if AP_TERRAIN_AVAILABLE && AC_TERRAIN    Location_Class::set_terrain(&terrain);    wp_nav.set_terrain(&terrain);#endif    wp_nav.set_avoidance(&avoid);    attitude_control.parameter_sanity_check();    pos_control.set_dt(MAIN_LOOP_SECONDS);    // init the optical flow sensor    init_optflow();#if MOUNT == ENABLED    // initialise camera mount    camera_mount.init(&DataFlash, serial_manager);#endif#if PRECISION_LANDING == ENABLED    // initialise precision landing    init_precland();#endif#ifdef USERHOOK_INIT    USERHOOK_INIT#endif#if CLI_ENABLED == ENABLED    if (g.cli_enabled) {        const char *msg = "\nPress ENTER 3 times to start interactive setup\n";        cliSerial->println(msg);        if (gcs[1].initialised && (gcs[1].get_uart() != nullptr)) {            gcs[1].get_uart()->println(msg);        }        if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != nullptr)) {            gcs[2].get_uart()->println(msg);        }    }#endif // CLI_ENABLED#if HIL_MODE != HIL_MODE_DISABLED    while (barometer.get_last_update() == 0) {        // the barometer begins updating when we get the first        // HIL_STATE message        gcs_send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message");        delay(1000);    }    // set INS to HIL mode    ins.set_hil_mode();#endif    // read Baro pressure at ground    //-----------------------------    init_barometer(true);    // initialise rangefinder    init_rangefinder();    // init proximity sensor    init_proximity();    // init beacons used for non-gps position estimation    init_beacon();    // initialise AP_RPM library    rpm_sensor.init();    // initialise mission library    mission.init();    // initialise the flight mode and aux switch    // ---------------------------    reset_control_switch();    init_aux_switches();    startup_INS_ground();    // set landed flags    set_land_complete(true);    set_land_complete_maybe(true);    // we don't want writes to the serial port to cause us to pause    // mid-flight, so set the serial ports non-blocking once we are    // ready to fly    serial_manager.set_blocking_writes_all(false);    // enable CPU failsafe    failsafe_enable();    ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));    ins.set_dataflash(&DataFlash);    cliSerial->print("\nReady to FLY ");    // flag that initialisation has completed    ap.initialised = true;}

请注意px4_setup();

void AP_BoardConfig::init(){#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN    px4_setup();#endif#if HAL_HAVE_IMU_HEATER    // let the HAL know the target temperature. We pass a pointer as    // we want the user to be able to change the parameter without    // rebooting    hal.util->set_imu_target_temp((int8_t *)&_imu_target_temperature);#endif}

请注意px4_setup_drivers();

/*  setup px4 peripherals and drivers */void AP_BoardConfig::px4_setup(){    px4_setup_peripherals();    px4_setup_pwm();    px4_setup_safety();    px4_setup_uart();    px4_setup_sbus();    px4_setup_drivers();    px4_setup_canbus();}

请注意
px4_start_driver(fmu_main, “fmu”, “sensor_reset 20”)
px4_start_common_sensors();
px4_start_fmuv2_sensors();

void AP_BoardConfig::px4_setup_drivers(void){#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4)    /*      this works around an issue with some FMUv4 hardware (eg. copies      of the Pixracer) which have incorrect components leading to      sensor brownout on boot     */    if (px4_start_driver(fmu_main, "fmu", "sensor_reset 20")) {        printf("FMUv4 sensor reset complete\n");            }#endif    // run board auto-detection    px4_autodetect();    if (px4.board_type == PX4_BOARD_PH2SLIM ||        px4.board_type == PX4_BOARD_PIXHAWK2) {        _imu_target_temperature.set_default(60);    }    if (px4.board_type == PX4_BOARD_PX4V1 ||        px4.board_type == PX4_BOARD_PHMINI ||        px4.board_type == PX4_BOARD_PH2SLIM ||        px4.board_type == PX4_BOARD_PIXRACER ||        px4.board_type == PX4_BOARD_PIXHAWK ||        px4.board_type == PX4_BOARD_PIXHAWK2) {        // use in-tree drivers        printf("Using in-tree drivers\n");        px4_configured_board = (enum px4_board_type)px4.board_type.get();        return;    }#if CONFIG_HAL_BOARD == HAL_BOARD_PX4    px4_start_common_sensors();    switch ((px4_board_type)px4.board_type.get()) {    case PX4_BOARD_AUTO:    default:        px4_start_fmuv1_sensors();        px4_start_fmuv2_sensors();        break;    }#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN    vrx_start_common_sensors();    switch ((px4_board_type)px4.board_type.get()) {    case VRX_BOARD_BRAIN51:        vrx_start_brain51_sensors();        break;    case VRX_BOARD_BRAIN52:        vrx_start_brain52_sensors();        break;    case VRX_BOARD_UBRAIN51:        vrx_start_ubrain51_sensors();        break;    case VRX_BOARD_UBRAIN52:        vrx_start_ubrain52_sensors();        break;    case VRX_BOARD_CORE10:        vrx_start_core10_sensors();        break;    case VRX_BOARD_BRAIN54:        vrx_start_brain54_sensors();        break;    default:        break;    }#endif // HAL_BOARD_PX4    px4_configured_board = (enum px4_board_type)px4.board_type.get();    // delay for 1 second to give time for drivers to initialise    hal.scheduler->delay(1000);}

请注意px4_start_driver(hmc5883_main, “hmc5883”, “-C -T -I -R 4 start”)等px4_start_driver()函数

void AP_BoardConfig::px4_start_fmuv2_sensors(void){#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)    bool have_FMUV3 = false;    printf("Starting FMUv2 sensors\n");    if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -I -R 4 start")) {        printf("Have internal hmc5883\n");    } else {        printf("No internal hmc5883\n");    }    // external MPU6000 is rotated YAW_180 from standard    if (px4_start_driver(mpu6000_main, "mpu6000", "-X -R 4 start")) {        printf("Found MPU6000 external\n");        have_FMUV3 = true;    } else {        if (px4_start_driver(mpu9250_main, "mpu9250", "-X -R 4 start")) {            printf("Found MPU9250 external\n");            have_FMUV3 = true;        } else {            printf("No MPU6000 or MPU9250 external\n");        }    }    if (have_FMUV3) {        // external L3GD20 is rotated YAW_180 from standard        if (px4_start_driver(l3gd20_main, "l3gd20", "-X -R 4 start")) {            printf("l3gd20 external started OK\n");        } else {            px4_sensor_error("No l3gd20");        }        // external LSM303D is rotated YAW_270 from standard        if (px4_start_driver(lsm303d_main, "lsm303d", "-a 16 -X -R 6 start")) {            printf("lsm303d external started OK\n");        } else {            px4_sensor_error("No lsm303d");        }        // internal MPU6000 is rotated ROLL_180_YAW_270 from standard        if (px4_start_driver(mpu6000_main, "mpu6000", "-R 14 start")) {            printf("Found MPU6000 internal\n");        } else {            if (px4_start_driver(mpu9250_main, "mpu9250", "-R 14 start")) {                printf("Found MPU9250 internal\n");            } else {                px4_sensor_error("No MPU6000 or MPU9250");            }        }        if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -S -R 8 start")) {            printf("Found SPI hmc5883\n");        }    } else {        // not FMUV3 (ie. not a pixhawk2)        if (px4_start_driver(mpu6000_main, "mpu6000", "start")) {            printf("Found MPU6000\n");        } else {            if (px4_start_driver(mpu9250_main, "mpu9250", "start")) {                printf("Found MPU9250\n");            } else {                printf("No MPU6000 or MPU9250\n");            }        }        if (px4_start_driver(l3gd20_main, "l3gd20", "start")) {            printf("l3gd20 started OK\n");        } else {            px4_sensor_error("no l3gd20 found");        }        if (px4_start_driver(lsm303d_main, "lsm303d", "-a 16 start")) {            printf("lsm303d started OK\n");        } else {            px4_sensor_error("no lsm303d found");        }    }    if (have_FMUV3) {        // on Pixhawk2 default IMU temperature to 60        _imu_target_temperature.set_default(60);    }    printf("FMUv2 sensors started\n");#endif // CONFIG_ARCH_BOARD_PX4FMU_V2}

px4_start_driver()函数原型

/*  start one px4 driver */bool AP_BoardConfig::px4_start_driver(main_fn_t main_function, const char *name, const char *arguments){    char *s = strdup(arguments);    char *args[10];    uint8_t nargs = 0;    char *saveptr = nullptr;    // parse into separate arguments    for (char *tok=strtok_r(s, " ", &saveptr); tok; tok=strtok_r(nullptr, " ", &saveptr)) {        args[nargs++] = tok;        if (nargs == ARRAY_SIZE(args)-1) {            break;        }    }    args[nargs++] = nullptr;    printf("Starting driver %s %s\n", name, arguments);    pid_t pid;    if (task_spawn(&pid, name, main_function, nullptr, nullptr,                   args, nullptr) != 0) {        free(s);        printf("Failed to spawn %s\n", name);        return false;    }    // wait for task to exit and gather status    int status = -1;    if (waitpid(pid, &status, 0) != pid) {        printf("waitpid failed for %s\n", name);        free(s);        return false;    }    free(s);    return (status >> 8) == 0;}

其中task_spawn(&pid, name, main_function, nullptr, nullptr, args, nullptr) != 0)创建了新的线程
比如px4_start_driver(hmc5883_main, “hmc5883”, “-C -T -I -R 4 start”)就映射到了px4的hmc5883驱动

inthmc5883_main(int argc, char *argv[]){    int ch;    enum HMC5883_BUS busid = HMC5883_BUS_ALL;    enum Rotation rotation = ROTATION_NONE;    bool calibrate = false;    bool temp_compensation = false;    while ((ch = getopt(argc, argv, "XISR:CT")) != EOF) {        switch (ch) {        case 'R':            rotation = (enum Rotation)atoi(optarg);            break;#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)        case 'I':            busid = HMC5883_BUS_I2C_INTERNAL;            break;#endif        case 'X':            busid = HMC5883_BUS_I2C_EXTERNAL;            break;        case 'S':            busid = HMC5883_BUS_SPI;            break;        case 'C':            calibrate = true;            break;        case 'T':            temp_compensation = true;            break;        default:            hmc5883::usage();            exit(0);        }    }    const char *verb = argv[optind];    /*     * Start/load the driver.     */    if (!strcmp(verb, "start")) {        hmc5883::start(busid, rotation);        if (calibrate && hmc5883::calibrate(busid) != 0) {            errx(1, "calibration failed");        }        if (temp_compensation) {            // we consider failing to setup temperature            // compensation as non-fatal            hmc5883::temp_enable(busid, true);        }        exit(0);    }    /*     * Test the driver/device.     */    if (!strcmp(verb, "test")) {        hmc5883::test(busid);    }    /*     * Reset the driver.     */    if (!strcmp(verb, "reset")) {        hmc5883::reset(busid);    }    /*     * enable/disable temperature compensation     */    if (!strcmp(verb, "tempoff")) {        hmc5883::temp_enable(busid, false);    }    if (!strcmp(verb, "tempon")) {        hmc5883::temp_enable(busid, true);    }    /*     * Print driver information.     */    if (!strcmp(verb, "info") || !strcmp(verb, "status")) {        hmc5883::info(busid);    }    /*     * Autocalibrate the scaling     */    if (!strcmp(verb, "calibrate")) {        if (hmc5883::calibrate(busid) == 0) {            errx(0, "calibration successful");        } else {            errx(1, "calibration failed");        }    }    errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate', 'tempoff', 'tempon' or 'info'");}

请注意hmc5883::start(busid, rotation);

/** * Start the driver. * * This function call only returns once the driver * is either successfully up and running or failed to start. */voidstart(enum HMC5883_BUS busid, enum Rotation rotation){    bool started = false;    for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {        if (busid == HMC5883_BUS_ALL && bus_options[i].dev != NULL) {            // this device is already started            continue;        }        if (busid != HMC5883_BUS_ALL && bus_options[i].busid != busid) {            // not the one that is asked for            continue;        }        started |= start_bus(bus_options[i], rotation);    }    if (!started) {        exit(1);    }}

请注意started |= start_bus(bus_options[i], rotation);

/** * start driver for a specific bus option */boolstart_bus(struct hmc5883_bus_option &bus, enum Rotation rotation){    if (bus.dev != nullptr) {        errx(1, "bus option already started");    }    device::Device *interface = bus.interface_constructor(bus.busnum);    if (interface->init() != OK) {        delete interface;        warnx("no device on bus %u", (unsigned)bus.busid);        return false;    }    bus.dev = new HMC5883(interface, bus.devpath, rotation);    if (bus.dev != nullptr && OK != bus.dev->init()) {        delete bus.dev;        bus.dev = NULL;        return false;    }    int fd = open(bus.devpath, O_RDONLY);    if (fd < 0) {        return false;    }    if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {        close(fd);        errx(1, "Failed to setup poll rate");    }    close(fd);    return true;}

请注意bus.dev->init()

intHMC5883::init(){    int ret = ERROR;    ret = CDev::init();    if (ret != OK) {        DEVICE_DEBUG("CDev init failed");        goto out;    }    /* allocate basic report buffers */    _reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));    if (_reports == nullptr) {        goto out;    }    /* reset the device configuration */    reset();    _class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);    ret = OK;    /* sensor is ok, but not calibrated */    _sensor_ok = true;out:    return ret;}

请注意_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);注册了设备路径。
返回bool start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation)
int fd = open(bus.devpath, O_RDONLY);
打开设备。此时就可以直接用read、ioctl等操作了。
此处有疑问了，是ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)读取的数据，还是ardupilot里面::read()读取数据

3.读取传感器数据

请注意fast_loop();

void Copter::loop(){    // wait for an INS sample    ins.wait_for_sample();    uint32_t timer = micros();    // check loop time    perf_info_check_loop_time(timer - fast_loopTimer);    // used by PI Loops    G_Dt                    = (float)(timer - fast_loopTimer) / 1000000.0f;    fast_loopTimer          = timer;    // for mainloop failure monitoring    mainLoop_count++;    // Execute the fast loop    // ---------------------    fast_loop();    // tell the scheduler one tick has passed    scheduler.tick();    // run all the tasks that are due to run. Note that we only    // have to call this once per loop, as the tasks are scheduled    // in multiples of the main loop tick. So if they don't run on    // the first call to the scheduler they won't run on a later    // call until scheduler.tick() is called again    uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros();    scheduler.run(time_available > MAIN_LOOP_MICROS ? 0u : time_available);}

请注意read_AHRS();

// Main loop - 400hzvoid Copter::fast_loop(){    // IMU DCM Algorithm    // --------------------    read_AHRS();    // run low level rate controllers that only require IMU data    attitude_control.rate_controller_run();#if FRAME_CONFIG == HELI_FRAME    update_heli_control_dynamics();#endif //HELI_FRAME    // send outputs to the motors library    motors_output();    // Inertial Nav    // --------------------    read_inertia();    // check if ekf has reset target heading or position    check_ekf_reset();    // run the attitude controllers    update_flight_mode();    // update home from EKF if necessary    update_home_from_EKF();    // check if we've landed or crashed    update_land_and_crash_detectors();#if MOUNT == ENABLED    // camera mount's fast update    camera_mount.update_fast();#endif    // log sensor health    if (should_log(MASK_LOG_ANY)) {        Log_Sensor_Health();    }}

请注意ahrs.update();

void Copter::read_AHRS(void){    // Perform IMU calculations and get attitude info    //-----------------------------------------------#if HIL_MODE != HIL_MODE_DISABLED    // update hil before ahrs update    gcs_check_input();#endif    ahrs.update();}

这个地方调用了AP_AHRS_NavEKF 中的update,而不是AP_AHRS_DCM 的updata

void AP_AHRS_NavEKF::update(void){#if !AP_AHRS_WITH_EKF1    if (_ekf_type == 1) {        _ekf_type.set(2);    }#endif    update_DCM();#if AP_AHRS_WITH_EKF1    update_EKF1();#endif    update_EKF2();#if CONFIG_HAL_BOARD == HAL_BOARD_SITL    update_SITL();#endif    // call AHRS_update hook if any    AP_Module::call_hook_AHRS_update(*this);}

这里update_DCM();直接调用了AP_AHRS_DCM 的updata

void AP_AHRS_NavEKF::update_DCM(void){    // we need to restore the old DCM attitude values as these are    // used internally in DCM to calculate error values for gyro drift    // correction    roll = _dcm_attitude.x;    pitch = _dcm_attitude.y;    yaw = _dcm_attitude.z;    update_cd_values();    AP_AHRS_DCM::update();    // keep DCM attitude available for get_secondary_attitude()    _dcm_attitude(roll, pitch, yaw);}

接着AP_AHRS_DCM::update();

// run a full DCM update roundvoidAP_AHRS_DCM::update(void){    float delta_t;    if (_last_startup_ms == 0) {        _last_startup_ms = AP_HAL::millis();    }    // tell the IMU to grab some data    _ins.update();    // ask the IMU how much time this sensor reading represents    delta_t = _ins.get_delta_time();    // if the update call took more than 0.2 seconds then discard it,    // otherwise we may move too far. This happens when arming motors    // in ArduCopter    if (delta_t > 0.2f) {        memset(&_ra_sum[0], 0, sizeof(_ra_sum));        _ra_deltat = 0;        return;    }    // Integrate the DCM matrix using gyro inputs    matrix_update(delta_t);    // Normalize the DCM matrix    normalize();    // Perform drift correction    drift_correction(delta_t);    // paranoid check for bad values in the DCM matrix    check_matrix();    // Calculate pitch, roll, yaw for stabilization and navigation    euler_angles();    // update trig values including _cos_roll, cos_pitch    update_trig();}

请注意

// tell the IMU to grab some data    _ins.update();

/*  update gyro and accel values from backends */void AP_InertialSensor::update(void){    // during initialisation update() may be called without    // wait_for_sample(), and a wait is implied    wait_for_sample();    if (!_hil_mode) {        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {            // mark sensors unhealthy and let update() in each backend            // mark them healthy via _publish_gyro() and            // _publish_accel()            _gyro_healthy[i] = false;            _accel_healthy[i] = false;            _delta_velocity_valid[i] = false;            _delta_angle_valid[i] = false;        }        for (uint8_t i=0; i<_backend_count; i++) {            _backends[i]->update();        }        // clear accumulators        for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {            _delta_velocity_acc[i].zero();            _delta_velocity_acc_dt[i] = 0;            _delta_angle_acc[i].zero();            _delta_angle_acc_dt[i] = 0;        }        if (!_startup_error_counts_set) {            for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {                _accel_startup_error_count[i] = _accel_error_count[i];                _gyro_startup_error_count[i] = _gyro_error_count[i];            }            if (_startup_ms == 0) {                _startup_ms = AP_HAL::millis();            } else if (AP_HAL::millis()-_startup_ms > 2000) {                _startup_error_counts_set = true;            }        }        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {            if (_accel_error_count[i] < _accel_startup_error_count[i]) {                _accel_startup_error_count[i] = _accel_error_count[i];            }            if (_gyro_error_count[i] < _gyro_startup_error_count[i]) {                _gyro_startup_error_count[i] = _gyro_error_count[i];            }        }        // adjust health status if a sensor has a non-zero error count        // but another sensor doesn't.        bool have_zero_accel_error_count = false;        bool have_zero_gyro_error_count = false;        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {            if (_accel_healthy[i] && _accel_error_count[i] <= _accel_startup_error_count[i]) {                have_zero_accel_error_count = true;            }            if (_gyro_healthy[i] && _gyro_error_count[i] <= _gyro_startup_error_count[i]) {                have_zero_gyro_error_count = true;            }        }        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {            if (_gyro_healthy[i] && _gyro_error_count[i] > _gyro_startup_error_count[i] && have_zero_gyro_error_count) {                // we prefer not to use a gyro that has had errors                _gyro_healthy[i] = false;            }            if (_accel_healthy[i] && _accel_error_count[i] > _accel_startup_error_count[i] && have_zero_accel_error_count) {                // we prefer not to use a accel that has had errors                _accel_healthy[i] = false;            }        }        // set primary to first healthy accel and gyro        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {            if (_gyro_healthy[i] && _use[i]) {                _primary_gyro = i;                break;            }        }        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {            if (_accel_healthy[i] && _use[i]) {                _primary_accel = i;                break;            }        }    }    _have_sample = false;}

请注意_backends[i]->update();
update()是虚函数
那么需要找update()所在类的继承类，搜索public AP_InertialSensor_Backend

有很多继承类，比如MPU6000

/*  publish any pending data */bool AP_InertialSensor_MPU6000::update(){    update_accel(_accel_instance);    update_gyro(_gyro_instance);    _publish_temperature(_accel_instance, _temp_filtered);    return true;}

px4

bool AP_InertialSensor_PX4::update(void) {    // get the latest sample from the sensor drivers    _get_sample();    for (uint8_t k=0; k<_num_accel_instances; k++) {        update_accel(_accel_instance[k]);    }    for (uint8_t k=0; k<_num_gyro_instances; k++) {        update_gyro(_gyro_instance[k]);    }    return true;}

那么需要找谁给_backends[i]赋了值
搜索_backends可找到_add_backend()

bool AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend){    if (!backend)        return false;    if (_backend_count == INS_MAX_BACKENDS)        AP_HAL::panic("Too many INS backends");    _backends[_backend_count++] = backend;    return true;}

那么看哪里调用了_add_backend()

/*  detect available backends for this board */voidAP_InertialSensor::detect_backends(void){    if (_backends_detected)        return;    _backends_detected = true;    if (_hil_mode) {        _add_backend(AP_InertialSensor_HIL::detect(*this));        return;    } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL    _add_backend(AP_InertialSensor_SITL::detect(*this)); #elif HAL_INS_DEFAULT == HAL_INS_HIL    _add_backend(AP_InertialSensor_HIL::detect(*this)); #elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI && defined(HAL_INS_DEFAULT_ROTATION)    _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME),                                                  HAL_INS_DEFAULT_ROTATION)); #elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI    _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME))); #elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C && defined(HAL_INS_DEFAULT_ROTATION)    _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR),                                                  HAL_INS_DEFAULT_ROTATION)); #elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C    _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR))); #elif HAL_INS_DEFAULT == HAL_INS_BH    _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR)));    _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME))); #elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN    if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PX4V1) {        _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));    } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK) {        if (!_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180))) {            // handle pixfalcon with mpu9250 instead of mpu6000            _fast_sampling_mask.set_default(1);            _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180));        }        _add_backend(AP_InertialSensor_LSM9DS0::probe(*this,                                                      hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),                                                      hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME),                                                      ROTATION_ROLL_180, ROTATION_ROLL_180_YAW_270));    } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) {        // older Pixhawk2 boards have the MPU6000 instead of MPU9250        _fast_sampling_mask.set_default(1);        if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_PITCH_180))) {            _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_EXT_NAME), ROTATION_PITCH_180));        }        _add_backend(AP_InertialSensor_LSM9DS0::probe(*this,                                                      hal.spi->get_device(HAL_INS_LSM9DS0_EXT_G_NAME),                                                      hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME),                                                      ROTATION_ROLL_180_YAW_270, ROTATION_ROLL_180_YAW_90));        if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270))) {            _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_YAW_270));        }    } ......

可以找到else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2)判断
AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_YAW_270)返回AP_InertialSensor_Backend类的对象，也就是操作传感器的后端对象
我们使用的是px4，所以可以找到update()的实例化
(先岔开一会，想看看哪里调用了void AP_InertialSensor::detect_backends(void)，请看最下面第4点)

AP_InertialSensor_PX4::update(void) 是update()的实现

bool AP_InertialSensor_PX4::update(void) {    // get the latest sample from the sensor drivers    _get_sample();    for (uint8_t k=0; k<_num_accel_instances; k++) {        update_accel(_accel_instance[k]);    }    for (uint8_t k=0; k<_num_gyro_instances; k++) {        update_gyro(_gyro_instance[k]);    }    return true;}

请注意_get_sample();

void AP_InertialSensor_PX4::_get_sample(){    for (uint8_t i=0; i<MAX(_num_accel_instances,_num_gyro_instances);i++) {        struct accel_report accel_report;        struct gyro_report gyro_report;        bool gyro_valid = _get_gyro_sample(i,gyro_report);        bool accel_valid = _get_accel_sample(i,accel_report);        while(gyro_valid || accel_valid) {            // interleave accel and gyro samples by time - this will allow sculling corrections later            // check the next gyro measurement to see if it needs to be integrated first            if(gyro_valid && accel_valid && gyro_report.timestamp <= accel_report.timestamp) {                _new_gyro_sample(i,gyro_report);                gyro_valid = _get_gyro_sample(i,gyro_report);                continue;            }            // if not, try to integrate an accelerometer sample            if(accel_valid) {                _new_accel_sample(i,accel_report);                accel_valid = _get_accel_sample(i,accel_report);                continue;            }            // if not, we've only got gyro samples left in the buffer            if(gyro_valid) {                _new_gyro_sample(i,gyro_report);                gyro_valid = _get_gyro_sample(i,gyro_report);            }        }    }}

请注意
bool gyro_valid = _get_gyro_sample(i,gyro_report);
bool accel_valid = _get_accel_sample(i,accel_report);

bool AP_InertialSensor_PX4::_get_gyro_sample(uint8_t i, struct gyro_report &gyro_report) {    if (i<_num_gyro_instances &&         _gyro_fd[i] != -1 &&         ::read(_gyro_fd[i], &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) &&         gyro_report.timestamp != _last_gyro_timestamp[i]) {        return true;    }    return false;}

请注意 ::read(_gyro_fd[i], &gyro_report, sizeof(gyro_report))
根据2中_class_instance = register_class_devname(PATH);注册设备路径，可以根据_gyro_fd描述符直接调用

ssize_tMPU6000::read(struct file *filp, char *buffer, size_t buflen){    unsigned count = buflen / sizeof(accel_report);    /* buffer must be large enough */    if (count < 1) {        return -ENOSPC;    }    /* if automatic measurement is not enabled, get a fresh measurement into the buffer */    if (_call_interval == 0) {        _accel_reports->flush();        measure();    }    /* if no data, error (we could block here) */    if (_accel_reports->empty()) {        return -EAGAIN;    }    perf_count(_accel_reads);    /* copy reports out of our buffer to the caller */    accel_report *arp = reinterpret_cast<accel_report *>(buffer);    int transferred = 0;    while (count--) {        if (!_accel_reports->get(arp)) {            break;        }        transferred++;        arp++;    }    /* return the number of bytes transferred */    return (transferred * sizeof(accel_report));}

于是读出数据。
疑问：是ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)读取的数据，还是ardupilot里面::read()读取数据？还是他们都可以读取数据？

4.startup_INS_ground()

void Copter::setup()    |    |init_ardupilot();    \     \__void Copter::init_ardupilot()            |            |        startup_INS_ground();            |            |        void Copter::startup_INS_ground()            |            \             \_____ins.init(scheduler.get_loop_rate_hz());                        |                        |               void AP_InertialSensor::init(uint16_t sample_rate)                        |                        |               void AP_InertialSensor::_start_backends()                        |                        |               void AP_InertialSensor::detect_backends(void)                        |                        |               _add_backend(AP_InertialSensor_PX4::detect(*this));                        |                        |                        \                         \_____AP_InertialSensor_Backend *AP_InertialSensor_PX4::detect(AP_InertialSensor &_imu)                                    |                                    |                            bool AP_InertialSensor_PX4::_init_sensor(void) 

如果您觉得此文对您的发展有用，请随意打赏。
您的鼓励将是笔者书写高质量文章的最大动力^_^!!