File content as of revision 8:c5dc1ce10722:

#include "mbed.h"
#include "magnetometer.h"
#include <quan/max.hpp>
#include <quan/min.hpp>
#include <quan/time.hpp>

void mag_adjusted_values(
    quan::time::ms const & update_rate, 
    quan::three_d::vect<double> const & gains,
    quan::three_d::vect<quan::magnetic_flux_density::uT> const & offsets
);
void mag_raw_values(quan::time::ms const & update_rate);
void mag_offset_calibrate();

namespace {
    
    QUAN_QUANTITY_LITERAL(time,ms);

    // terminal loop, printing message periodically
    void loop_forever(std::string const & str)
    {
        DigitalOut led1(LED1,1);
        // stop but print error dynamically
        int count = 0;
        for (;;) {
            led1 = 1;
            std::cout << str << " " << count++ << '\n';
            ThisThread::sleep_for(200U);
            led1 = 0;
            ThisThread::sleep_for(800U);
        }
    }
    
    // do something with mag output
    void callback(quan::three_d::vect<quan::magnetic_flux_density::uT> const & v)
    {
         std::cout << "val = " << v << '\n';
    }
    
}// namespace

int main()
{
    DigitalOut led2(LED2,1);
    std::cout << "magnetometer test\n";

    if ( !mag_init()){
        loop_forever("failed to init magnetometer\n");
    }
    
    uint64_t constexpr update_rate_ms = 20U;
        
    auto prev_led = Kernel::get_ms_count();
    auto now = Kernel::get_ms_count();
    // TODO Thread for this
    for (;;){
        mag_start_measurement();
        do{
           ThisThread::sleep_for(10U);
        }  while ( ! mag_data_ready() ); 
        
        quan::three_d::vect<quan::magnetic_flux_density::uT> v;
        if (mag_read(v)){
          callback(v);
        }
        if ( (now - prev_led) >= 500U){
            prev_led = now;
            led2 = (led2 == 0) ? 1: 0;
        }
        auto const next_wake = now +
            static_cast<uint64_t>(update_rate_ms);
        ThisThread::sleep_until(next_wake);
        now = next_wake;
    }
}