C++ Compass类代码示例

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);
}

int main() {
    
    Compass testCompass;
    InterfaceKit ifKit;
    LCD lcd;
    Motor motor;
    Spatial spatial;

    CPhidgetManagerHandle device = 0;
    LocalErrorCatcher(
        CPhidgetManager_create(&device));
    
    LocalErrorCatcher(
        CPhidgetManager_set_OnAttach_Handler((CPhidgetManagerHandle) device, 
                                         AttachHandler, NULL));
    
    LocalErrorCatcher(
        CPhidgetManager_set_OnDetach_Handler((CPhidgetManagerHandle ) device,
                                         DetachHandler, NULL));
    
    LocalErrorCatcher(
        CPhidgetManager_set_OnError_Handler((CPhidgetManagerHandle) device,
                                        LibraryErrorHandler, NULL));
    printf("Starting Phidget Playground...\n");
    // Most opening and closing would be via a cast to
    // (CPhidgetHandle), however, this manager has its
    // own handle struct to cast to.
	
	LocalErrorCatcher(
        CPhidgetManager_open((CPhidgetManagerHandle) device));
    
    std::stringstream ss;
    lcd.clear();
	
    for(int i=0;i<1000;i++){
        testCompass.refresh();
        ss << std::fixed <<  std::setprecision(1) << "Heading: " << testCompass.getHeading();
        lcd.setText(ss.str(), 0);
        ss.str(std::string());
        
        ss << std::fixed << std::setprecision(2) << spatial.getAcceleration(AXIS_X) << " " << spatial.getAcceleration(AXIS_Y) << " " << spatial.getAcceleration(AXIS_Z);
        lcd.setText(ss.str(), 1);
        ss.str(std::string());
        
        usleep(100000);
    }

    printf("Press Enter to end...\n");
    getchar();

    LocalErrorCatcher(
        CPhidgetManager_close((CPhidgetManagerHandle) device));
    LocalErrorCatcher(
        CPhidgetManager_delete((CPhidgetManagerHandle) device));

    return 0;
}

int main()
{

	irr::IrrlichtDevice* device = irr::createDevice(irr::video::EDT_OPENGL, irr::core::dimension2du(1024, 768));
	if (device == 0)
		return 1;

	irr::gui::IGUIEnvironment* guienv = device->getGUIEnvironment();
	irr::video::IVideoDriver* driver = device->getVideoDriver();
	irr::scene::ISceneManager* smgr = device->getSceneManager();

	guienv->addStaticText(L"Press ALT + F4 to exit", irr::core::rect<irr::s32>(20, 20, 200, 40));

	irr::scene::ICameraSceneNode* camera = smgr->addCameraSceneNodeFPS();


	//an empty scene is boring
	for (irr::u32 i = 0; i < 10; ++i)
		smgr->addCubeSceneNode(10.0f, 0, -1, irr::core::vector3df(10.0f*i + 10, 0.0f, 50.0));


	// add a compass
	const irr::core::dimension2di compassSize(128, 128);

	irr::core::rect<irr::s32> compassRect;
	compassRect.UpperLeftCorner.X = 880;
	compassRect.UpperLeftCorner.Y = 10;
	compassRect.LowerRightCorner.X = 880 + compassSize.Width;
	compassRect.LowerRightCorner.Y = 10 + compassSize.Height;

	Compass* compass = new Compass(compassRect, guienv, guienv->getRootGUIElement());
	compass->SetCompassBodyTexture(driver->getTexture("compass_body.png"));
	compass->SetCompassNeedleTexture(driver->getTexture("compass_needle.png"));


	while (device->run())
	{
		//update compass
		irr::core::vector3df vec(0.0f, 0.0f, 1.0f);
		camera->getAbsoluteTransformation().rotateVect(vec);
		compass->SetCompassHeading(vec.getHorizontalAngle().Y);

		driver->beginScene(true, true, irr::video::SColor(255, 128, 128, 128));
		smgr->drawAll();
		guienv->drawAll();
		driver->endScene();
	}

	device->drop();
	compass->drop();

	return 0;
}

m2::PointF LayerCacher::CacheCompass(Position const & position, ref_ptr<LayerRenderer> renderer,
                                     ref_ptr<dp::TextureManager> textures)
{
  m2::PointF compassSize;
  Compass compass = Compass(position);
  drape_ptr<ShapeRenderer> shape = compass.Draw(compassSize, textures, bind(&DrapeGui::CallOnCompassTappedHandler,
                                                                            &DrapeGui::Instance()));

  renderer->AddShapeRenderer(WIDGET_COMPASS, move(shape));

  return compassSize;
}

void SensorfwCompass::slotDataAvailable(const Compass& data)
{
    // The scale for level is [0,3], where 3 is the best
    // Qt: Measured as a value from 0 to 1 with higher values being better.
    m_reading.setCalibrationLevel(((float) data.level()) / 3.0);

    // The scale for degrees from sensord is [0,359]
    // Value can be directly used as azimuth
    m_reading.setAzimuth(data.degrees());

    m_reading.setTimestamp(data.data().timestamp_);
    newReadingAvailable();
}

void setup() {
    hal.console->println("Compass library test");

    if (!compass.init()) {
        hal.console->println("compass initialisation failed!");
        while (1) ;
    }
    hal.console->printf("init done - %u compasses detected\n", compass.get_count());

    compass.set_and_save_offsets(0,0,0,0); // set offsets to account for surrounding interference
    compass.set_declination(ToRad(0.0f)); // set local difference between magnetic north and true north

    hal.scheduler->delay(1000);
    timer = AP_HAL::micros();
}

// to be called only once on boot for initializing objects
static void setup()
{
    hal.console->printf("Compass library test\n");

    board_config.init();
    vehicle.ahrs.init();
    compass.init();
    hal.console->printf("init done - %u compasses detected\n", compass.get_count());

    // set offsets to account for surrounding interference
    compass.set_and_save_offsets(0, Vector3f(0, 0, 0));
    // set local difference between magnetic north and true north
    compass.set_declination(ToRad(0.0f));

    hal.scheduler->delay(1000);
    timer = AP_HAL::micros();
}

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
}

static void loop()
{
    static const uint8_t compass_count = compass.get_count();
    static float min[COMPASS_MAX_INSTANCES][3];
    static float max[COMPASS_MAX_INSTANCES][3];
    static float offset[COMPASS_MAX_INSTANCES][3];

    compass.accumulate();

    if ((AP_HAL::micros() - timer) > 100000L) {
        timer = AP_HAL::micros();
        compass.read();
        unsigned long read_time = AP_HAL::micros() - timer;

        for (uint8_t i = 0; i < compass_count; i++) {
            float heading;

            hal.console->printf("Compass #%u: ", i);

            if (!compass.healthy()) {
                hal.console->println("not healthy");
                continue;
            }

            Matrix3f dcm_matrix;
            // use roll = 0, pitch = 0 for this example
            dcm_matrix.from_euler(0, 0, 0);
            heading = compass.calculate_heading(dcm_matrix, i);
            compass.learn_offsets();

            const Vector3f &mag = compass.get_field(i);

            // capture min
            min[i][0] = MIN(mag.x, min[i][0]);
            min[i][1] = MIN(mag.y, min[i][1]);
            min[i][2] = MIN(mag.z, min[i][2]);

            // capture max
            max[i][0] = MAX(mag.x, max[i][0]);
            max[i][1] = MAX(mag.y, max[i][1]);
            max[i][2] = MAX(mag.z, max[i][2]);

            // calculate offsets
            offset[i][0] = -(max[i][0] + min[i][0]) / 2;
            offset[i][1] = -(max[i][1] + min[i][1]) / 2;
            offset[i][2] = -(max[i][2] + min[i][2]) / 2;

            // display all to user
            hal.console->printf("Heading: %.2f (%3d,%3d,%3d)",
                                ToDeg(heading),
                                (int)mag.x,
                                (int)mag.y,
                                (int)mag.z);

            // display offsets
            hal.console->printf(" offsets(%.2f, %.2f, %.2f)",
                                offset[i][0], offset[i][1], offset[i][2]);

            hal.console->printf(" t=%u", (unsigned)read_time);

            hal.console->println();
        }
    } else {
        hal.scheduler->delay(1);
    }
}

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);
}

extern "C" int report_heading() {
//	return Compass::getInstance().reportHeading();
	return compass.reportHeading();
}

// loop
static void loop()
{
    static const uint8_t compass_count = compass.get_count();
    static float min[COMPASS_MAX_INSTANCES][3];
    static float max[COMPASS_MAX_INSTANCES][3];
    static float offset[COMPASS_MAX_INSTANCES][3];

    // run read() at 10Hz
    if ((AP_HAL::micros() - timer) > 100000L) {
        timer = AP_HAL::micros();
        compass.read();
        const uint32_t read_time = AP_HAL::micros() - timer;

        for (uint8_t i = 0; i < compass_count; i++) {
            float heading;

            hal.console->printf("Compass #%u: ", i);

            if (!compass.healthy()) {
                hal.console->printf("not healthy\n");
                continue;
            }

            Matrix3f dcm_matrix;
            // use roll = 0, pitch = 0 for this example
            dcm_matrix.from_euler(0, 0, 0);
            heading = compass.calculate_heading(dcm_matrix, i);

            const Vector3f &mag = compass.get_field(i);

            // capture min
            min[i][0] = MIN(mag.x, min[i][0]);
            min[i][1] = MIN(mag.y, min[i][1]);
            min[i][2] = MIN(mag.z, min[i][2]);

            // capture max
            max[i][0] = MAX(mag.x, max[i][0]);
            max[i][1] = MAX(mag.y, max[i][1]);
            max[i][2] = MAX(mag.z, max[i][2]);

            // calculate offsets
            offset[i][0] = -(max[i][0] + min[i][0]) / 2;
            offset[i][1] = -(max[i][1] + min[i][1]) / 2;
            offset[i][2] = -(max[i][2] + min[i][2]) / 2;

            // display all to user
            hal.console->printf("Heading: %.2f (%3d, %3d, %3d)",
                                (double)ToDeg(heading),
                                (int)mag.x,
                                (int)mag.y,
                                (int)mag.z);

            // display offsets
            hal.console->printf(" offsets(%.2f, %.2f, %.2f)",
                                (double)offset[i][0],
                                (double)offset[i][1],
                                (double)offset[i][2]);

            hal.console->printf(" t=%u", (unsigned)read_time);

            hal.console->printf("\n");
        }
    } else {

        // if stipulated time has not passed between two distinct readings, delay the program for a millisecond
        hal.scheduler->delay(1);
    }
}

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 loop(void)
{
    static uint16_t counter;
    static uint32_t last_t, last_print, last_compass;
    uint32_t now = hal.scheduler->micros();
    float heading = 0;

    if (last_t == 0) {
        last_t = now;
        return;
    }
    last_t = now;

    if (now - last_compass > 100*1000UL &&
        compass.read()) {
        heading = compass.calculate_heading(ahrs.get_dcm_matrix());
        // read compass at 10Hz
        last_compass = now;
#if WITH_GPS
        g_gps->update();
#endif
    }

    ahrs.update();
    counter++;

    if (now - last_print >= 100000 /* 100ms : 10hz */) {
        Vector3f drift  = ahrs.get_gyro_drift();
        hal.console->printf(
                "r:%4.1f  p:%4.1f y:%4.1f "
                    "drift=(%5.1f %5.1f %5.1f) hdg=%.1f rate=%.1f\n",
                        ToDeg(ahrs.roll),
                        ToDeg(ahrs.pitch),
                        ToDeg(ahrs.yaw),
                        ToDeg(drift.x),
                        ToDeg(drift.y),
                        ToDeg(drift.z),
                        compass.use_for_yaw() ? ToDeg(heading) : 0.0f,
                        (1.0e6f*counter)/(now-last_print));
        last_print = now;
        counter = 0;
    }
}

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);
}