ardupilot如何读取传感器数据

最新推荐文章于 2024-02-15 15:24:21 发布

虾米一代

最新推荐文章于 2024-02-15 15:24:21 发布

阅读量4.8k

点赞数 1

分类专栏：四轴飞行器

本文链接：https://blog.csdn.net/czyv587/article/details/77171608

版权

四轴飞行器专栏收录该内容

34 篇文章 31 订阅

订阅专栏

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

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驱动

int
hmc5883_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.
 */
void
start(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
 */
bool
start_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()

int
HMC5883::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 - 400hz
void 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 round
void
AP_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
 */
void
AP_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_t
MPU6000::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)

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

虾米一代

关注

1
点赞
踩
14

收藏

觉得还不错? 一键收藏
1
评论
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.
复制链接

扫一扫