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();
}
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();
}
// 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 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 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 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);
}
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();
}
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);
}
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 loop()
{
static float min[3], max[3], offset[3];
compass.accumulate();
if((AP_HAL::micros()- timer) > 100000L)
{
timer = AP_HAL::micros();
compass.read();
unsigned long read_time = AP_HAL::micros() - timer;
float heading;
if (!compass.healthy()) {
hal.console->println("not healthy");
return;
}
Matrix3f dcm_matrix;
// use roll = 0, pitch = 0 for this example
dcm_matrix.from_euler(0, 0, 0);
heading = compass.calculate_heading(dcm_matrix);
compass.learn_offsets();
// capture min
const Vector3f &mag = compass.get_field();
if( mag.x < min[0] )
min[0] = mag.x;
if( mag.y < min[1] )
min[1] = mag.y;
if( mag.z < min[2] )
min[2] = mag.z;
// capture max
if( mag.x > max[0] )
max[0] = mag.x;
if( mag.y > max[1] )
max[1] = mag.y;
if( mag.z > max[2] )
max[2] = mag.z;
// calculate offsets
offset[0] = -(max[0]+min[0])/2;
offset[1] = -(max[1]+min[1])/2;
offset[2] = -(max[2]+min[2])/2;
// display all to user
hal.console->printf("Heading: %.2f (%3d,%3d,%3d) i2c error: %u",
ToDeg(heading),
(int)mag.x,
(int)mag.y,
(int)mag.z,
(unsigned)hal.i2c->lockup_count());
// display offsets
hal.console->printf(" offsets(%.2f, %.2f, %.2f)",
offset[0], offset[1], offset[2]);
hal.console->printf(" t=%u", (unsigned)read_time);
hal.console->println();
} else {
hal.scheduler->delay(1);
}
}
void MotorControl::update_omega(void) {
pid->setProcessValue(_compass->measure_angle(target_angle_));
omega = pid->compute();
move(power_, move_angle_);
}
