自学教程：C++ AP_InertialSensor类代码示例

bool AP_Arming::ins_gyros_consistent(const AP_InertialSensor &ins){    const uint8_t gyro_count = ins.get_gyro_count();    if (gyro_count <= 1) {        return true;    }    const Vector3f &prime_gyro_vec = ins.get_gyro();    const uint32_t now = AP_HAL::millis();    for(uint8_t i=0; i<gyro_count; i++) {        if (!ins.use_gyro(i)) {            continue;        }        // get next gyro vector        const Vector3f &gyro_vec = ins.get_gyro(i);        Vector3f vec_diff = gyro_vec - prime_gyro_vec;        // allow for up to 5 degrees/s difference. Pass if it has        // been OK in last 10 seconds        if (vec_diff.length() <= radians(5)) {            last_gyro_pass_ms[i] = now;        }        if (now - last_gyro_pass_ms[i] > 10000) {            return false;        }    }    return true;}

void GCS_MAVLINK::send_sensor_offsets(const AP_InertialSensor &ins, const Compass &compass, AP_Baro &barometer){    // run this message at a much lower rate - otherwise it    // pointlessly wastes quite a lot of bandwidth    static uint8_t counter;    if (counter++ < 10) {        return;    }    counter = 0;    const Vector3f &mag_offsets = compass.get_offsets(0);    const Vector3f &accel_offsets = ins.get_accel_offsets(0);    const Vector3f &gyro_offsets = ins.get_gyro_offsets(0);    mavlink_msg_sensor_offsets_send(chan,                                    mag_offsets.x,                                    mag_offsets.y,                                    mag_offsets.z,                                    compass.get_declination(),                                    barometer.get_pressure(),                                    barometer.get_temperature()*100,                                    gyro_offsets.x,                                    gyro_offsets.y,                                    gyro_offsets.z,                                    accel_offsets.x,                                    accel_offsets.y,                                    accel_offsets.z);}

/*  send LOCAL_POSITION_NED message */void GCS_MAVLINK::send_vibration(const AP_InertialSensor &ins) const{    Vector3f vibration = ins.get_vibration_levels();    mavlink_msg_vibration_send(        chan,        AP_HAL::micros64(),        vibration.x,        vibration.y,        vibration.z,        ins.get_accel_clip_count(0),        ins.get_accel_clip_count(1),        ins.get_accel_clip_count(2));}

/*  send LOCAL_POSITION_NED message */void GCS_MAVLINK::send_vibration(const AP_InertialSensor &ins) const{#if INS_VIBRATION_CHECK    Vector3f vibration = ins.get_vibration_levels();    mavlink_msg_vibration_send(        chan,        hal.scheduler->micros64(),        vibration.x,        vibration.y,        vibration.z,        ins.get_accel_clip_count(0),        ins.get_accel_clip_count(1),        ins.get_accel_clip_count(2));#endif}

void SchedTest::setup(void){    // we     ins.init(AP_InertialSensor::COLD_START, 			 AP_InertialSensor::RATE_50HZ);    // initialise the scheduler    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));}

/* MPU6000 implementation of the HMC5843 */AP_HMC5843_SerialBus_MPU6000::AP_HMC5843_SerialBus_MPU6000(AP_InertialSensor &ins,                                                           uint8_t addr){    // Only initialize members. Fails are handled by configure or while    // getting the semaphore    _bus = ins.get_auxiliar_bus(HAL_INS_MPU60XX_SPI);    if (!_bus)        return;    _slave = _bus->request_next_slave(addr);}

void setup(void){    // setup any board specific drivers    BoardConfig.init();    hal.console->printf("AP_InertialSensor startup.../n");    ins.init(100);    // display initial values    display_offsets_and_scaling();    // display number of detected accels/gyros    hal.console->printf("/n");    hal.console->printf("Number of detected accels : %u/n", ins.get_accel_count());    hal.console->printf("Number of detected gyros  : %u/n/n", ins.get_gyro_count());    hal.console->printf("Complete. Reading:/n");}

static void run_calibration(){    float roll_trim, pitch_trim;    // clear off any other characters (like line feeds,etc)    while( hal.console->available() ) {        hal.console->read();    }    AP_InertialSensor_UserInteractStream interact(hal.console);    ins.calibrate_accel(&interact, roll_trim, pitch_trim);}

bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins){    const uint8_t accel_count = ins.get_accel_count();    if (accel_count <= 1) {        return true;    }    const Vector3f &prime_accel_vec = ins.get_accel();    const uint32_t now = AP_HAL::millis();    for(uint8_t i=0; i<accel_count; i++) {        if (!ins.use_accel(i)) {            continue;        }        // get next accel vector        const Vector3f &accel_vec = ins.get_accel(i);        Vector3f vec_diff = accel_vec - prime_accel_vec;        // allow for user-defined difference, typically 0.75 m/s/s. Has to pass in last 10 seconds        float threshold = accel_error_threshold;        if (i >= 2) {            /*              we allow for a higher threshold for IMU3 as it              runs at a different temperature to IMU1/IMU2,              and is not used for accel data in the EKF            */            threshold *= 3;        }        // EKF is less sensitive to Z-axis error        vec_diff.z *= 0.5f;        if (vec_diff.length() <= threshold) {            last_accel_pass_ms[i] = now;        }        if (now - last_accel_pass_ms[i] > 10000) {            return false;        }    }    return true;}

/* HMC5843 on an auxiliary bus of IMU driver */AP_HMC5843_BusDriver_Auxiliary::AP_HMC5843_BusDriver_Auxiliary(AP_InertialSensor &ins, uint8_t backend_id,                                                               uint8_t addr){    /*     * Only initialize members. Fails are handled by configure or while     * getting the semaphore     */    _bus = ins.get_auxiliary_bus(backend_id);    if (!_bus) {        return;    }    _slave = _bus->request_next_slave(addr);}

void setup(void){    hal.console->println("AP_InertialSensor startup...");#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_MEGA2560    // we need to stop the barometer from holding the SPI bus    hal.gpio->pinMode(40, HAL_GPIO_OUTPUT);    hal.gpio->write(40, 1);#endif    ins.init(AP_InertialSensor::COLD_START, 			 AP_InertialSensor::RATE_100HZ);    // display initial values    display_offsets_and_scaling();    hal.console->println("Complete. Reading:");}

void setup(void){#ifdef APM2_HARDWARE    // we need to stop the barometer from holding the SPI bus    hal.gpio->pinMode(40, HAL_HAL_GPIO_OUTPUT);    hal.gpio->write(40, HIGH);#endif    ins.init(AP_InertialSensor::RATE_100HZ);    ahrs.init();    serial_manager.init();    if( compass.init() ) {        hal.console->printf("Enabling compass/n");        ahrs.set_compass(&compass);    } else {        hal.console->printf("No compass detected/n");    }    gps.init(NULL, serial_manager);}

void GCS_MAVLINK::send_raw_imu(const AP_InertialSensor &ins, const Compass &compass){    const Vector3f &accel = ins.get_accel(0);    const Vector3f &gyro = ins.get_gyro(0);    const Vector3f &mag = compass.get_field(0);    mavlink_msg_raw_imu_send(        chan,        hal.scheduler->micros(),        accel.x * 1000.0f / GRAVITY_MSS,        accel.y * 1000.0f / GRAVITY_MSS,        accel.z * 1000.0f / GRAVITY_MSS,        gyro.x * 1000.0f,        gyro.y * 1000.0f,        gyro.z * 1000.0f,        mag.x,        mag.y,        mag.z);#if INS_MAX_INSTANCES > 1    if (ins.get_gyro_count() <= 1 &&        ins.get_accel_count() <= 1 &&        compass.get_count() <= 1) {        return;    }    const Vector3f &accel2 = ins.get_accel(1);    const Vector3f &gyro2 = ins.get_gyro(1);    const Vector3f &mag2 = compass.get_field(1);    mavlink_msg_scaled_imu2_send(        chan,        hal.scheduler->millis(),        accel2.x * 1000.0f / GRAVITY_MSS,        accel2.y * 1000.0f / GRAVITY_MSS,        accel2.z * 1000.0f / GRAVITY_MSS,        gyro2.x * 1000.0f,        gyro2.y * 1000.0f,        gyro2.z * 1000.0f,        mag2.x,        mag2.y,        mag2.z);        #endif}

void GCS_MAVLINK::send_raw_imu(const AP_InertialSensor &ins, const Compass &compass){    const Vector3f &accel = ins.get_accel(0);    const Vector3f &gyro = ins.get_gyro(0);    Vector3f mag;    if (compass.get_count() >= 1) {        mag = compass.get_field(0);    } else {        mag.zero();    }    mavlink_msg_raw_imu_send(        chan,        AP_HAL::micros(),        accel.x * 1000.0f / GRAVITY_MSS,        accel.y * 1000.0f / GRAVITY_MSS,        accel.z * 1000.0f / GRAVITY_MSS,        gyro.x * 1000.0f,        gyro.y * 1000.0f,        gyro.z * 1000.0f,        mag.x,        mag.y,        mag.z);    if (ins.get_gyro_count() <= 1 &&            ins.get_accel_count() <= 1 &&            compass.get_count() <= 1) {        return;    }    const Vector3f &accel2 = ins.get_accel(1);    const Vector3f &gyro2 = ins.get_gyro(1);    if (compass.get_count() >= 2) {        mag = compass.get_field(1);    } else {        mag.zero();    }    mavlink_msg_scaled_imu2_send(        chan,        AP_HAL::millis(),        accel2.x * 1000.0f / GRAVITY_MSS,        accel2.y * 1000.0f / GRAVITY_MSS,        accel2.z * 1000.0f / GRAVITY_MSS,        gyro2.x * 1000.0f,        gyro2.y * 1000.0f,        gyro2.z * 1000.0f,        mag.x,        mag.y,        mag.z);    if (ins.get_gyro_count() <= 2 &&            ins.get_accel_count() <= 2 &&            compass.get_count() <= 2) {        return;    }    const Vector3f &accel3 = ins.get_accel(2);    const Vector3f &gyro3 = ins.get_gyro(2);    if (compass.get_count() >= 3) {        mag = compass.get_field(2);    } else {        mag.zero();    }    mavlink_msg_scaled_imu3_send(        chan,        AP_HAL::millis(),        accel3.x * 1000.0f / GRAVITY_MSS,        accel3.y * 1000.0f / GRAVITY_MSS,        accel3.z * 1000.0f / GRAVITY_MSS,        gyro3.x * 1000.0f,        gyro3.y * 1000.0f,        gyro3.z * 1000.0f,        mag.x,        mag.y,        mag.z);}

void ReplayVehicle::setup(void) {    load_parameters();        // we pass zero log structures, as we will be outputting the log    // structures we need manually, to prevent FMT duplicates    dataflash.Init(log_structure, 0);    dataflash.StartNewLog();    ahrs.set_compass(&compass);    ahrs.set_fly_forward(true);    ahrs.set_wind_estimation(true);    ahrs.set_correct_centrifugal(true);    ahrs.set_ekf_use(true);    EKF2.set_enable(true);                            printf("Starting disarmed/n");    hal.util->set_soft_armed(false);    barometer.init();    barometer.setHIL(0);    barometer.update();    compass.init();    ins.set_hil_mode();}

void setup(void){    serial_manager.init();    ins.init(100);    baro.init();    ahrs.init();    gps.init(NULL, serial_manager);}

static void display_offsets_and_scaling(){    const Vector3f &accel_offsets = ins.get_accel_offsets();    const Vector3f &accel_scale = ins.get_accel_scale();    const Vector3f &gyro_offsets = ins.get_gyro_offsets();    // display results    hal.console->printf("/nAccel Offsets X:%10.8f /t Y:%10.8f /t Z:%10.8f/n",                        (double)accel_offsets.x,                        (double)accel_offsets.y,                        (double)accel_offsets.z);    hal.console->printf("Accel Scale X:%10.8f /t Y:%10.8f /t Z:%10.8f/n",                        (double)accel_scale.x,                        (double)accel_scale.y,                        (double)accel_scale.z);    hal.console->printf("Gyro Offsets X:%10.8f /t Y:%10.8f /t Z:%10.8f/n",                        (double)gyro_offsets.x,                        (double)gyro_offsets.y,                        (double)gyro_offsets.z);}

void SchedTest::loop(void){    // wait for an INS sample    ins.wait_for_sample();    // tell the scheduler one tick has passed    scheduler.tick();    // run all tasks that fit in 20ms    scheduler.run(20000);}

static void run_test(){    Vector3f accel;    Vector3f gyro;    float length;	uint8_t counter = 0;    // flush any user input    while( hal.console->available() ) {        hal.console->read();    }    // clear out any existing samples from ins    ins.update();    // loop as long as user does not press a key    while( !hal.console->available() ) {        // wait until we have a sample        ins.wait_for_sample();        // read samples from ins        ins.update();        accel = ins.get_accel();        gyro = ins.get_gyro();        length = accel.length();		if (counter++ % 50 == 0) {			// display results			hal.console->printf_P(PSTR("Accel X:%4.2f /t Y:%4.2f /t Z:%4.2f /t len:%4.2f /t Gyro X:%4.2f /t Y:%4.2f /t Z:%4.2f/n"), 								  accel.x, accel.y, accel.z, length, gyro.x, gyro.y, gyro.z);		}    }    // clear user input    while( hal.console->available() ) {        hal.console->read();    }}

void setup(void){    ins.init(100);    ahrs.init();    serial_manager.init();    if( compass.init() ) {        hal.console->printf("Enabling compass/n");        ahrs.set_compass(&compass);    } else {        hal.console->printf("No compass detected/n");    }    gps.init(NULL, serial_manager);}

static void display_offsets_and_scaling(){    Vector3f accel_offsets = ins.get_accel_offsets();    Vector3f accel_scale = ins.get_accel_scale();    Vector3f gyro_offsets = ins.get_gyro_offsets();    // display results    hal.console->printf_P(            PSTR("/nAccel Offsets X:%10.8f /t Y:%10.8f /t Z:%10.8f/n"),                    accel_offsets.x,                    accel_offsets.y,                    accel_offsets.z);    hal.console->printf_P(            PSTR("Accel Scale X:%10.8f /t Y:%10.8f /t Z:%10.8f/n"),                    accel_scale.x,                    accel_scale.y,                    accel_scale.z);    hal.console->printf_P(            PSTR("Gyro Offsets X:%10.8f /t Y:%10.8f /t Z:%10.8f/n"),                    gyro_offsets.x,                    gyro_offsets.y,                    gyro_offsets.z);}

void ReplayVehicle::setup(void) {    dataflash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0]));    dataflash.StartNewLog();    ahrs.set_compass(&compass);    ahrs.set_fly_forward(true);    ahrs.set_wind_estimation(true);    ahrs.set_correct_centrifugal(true);    ahrs.set_ekf_use(true);    printf("Starting disarmed/n");    hal.util->set_soft_armed(false);    barometer.init();    barometer.setHIL(0);    barometer.update();    compass.init();    ins.set_hil_mode();}

/*  update inertial sensor, reading data  */void SchedTest::ins_update(void){    ins_counter++;    ins.update();}

static void run_test(){    Vector3f accel;    Vector3f gyro;    uint8_t counter = 0;    static uint8_t accel_count = ins.get_accel_count();    static uint8_t gyro_count = ins.get_gyro_count();    static uint8_t ins_count = MAX(accel_count, gyro_count);    // flush any user input    while (hal.console->available()) {        hal.console->read();    }    // clear out any existing samples from ins    ins.update();    // loop as long as user does not press a key    while (!hal.console->available()) {        // wait until we have a sample        ins.wait_for_sample();        // read samples from ins        ins.update();        // print each accel/gyro result every 50 cycles        if (counter++ % 50 != 0) {            continue;        }        // loop and print each sensor        for (uint8_t ii = 0; ii < ins_count; ii++) {            char state;            if (ii > accel_count - 1) {                // No accel present                state = '-';            } else if (ins.get_accel_health(ii)) {                // Healthy accel                state = 'h';            } else {                // Accel present but not healthy                state = 'u';            }            accel = ins.get_accel(ii);            hal.console->printf("%u - Accel (%c) : X:%6.2f Y:%6.2f Z:%6.2f norm:%5.2f",                                ii, state, (double)accel.x, (double)accel.y, (double)accel.z,                                (double)accel.length());            gyro = ins.get_gyro(ii);            if (ii > gyro_count - 1) {                // No gyro present                state = '-';            } else if (ins.get_gyro_health(ii)) {                // Healthy gyro                state = 'h';            } else {                // Gyro present but not healthy                state = 'u';            }            hal.console->printf("   Gyro (%c) : X:%6.2f Y:%6.2f Z:%6.2f/n",                                state, (double)gyro.x, (double)gyro.y, (double)gyro.z);            auto temp = ins.get_temperature(ii);            hal.console->printf("   t:%6.2f/n", (double)temp);        }    }    // clear user input    while (hal.console->available()) {        hal.console->read();    }}