First draft HMC5883 magnetometer sensor using physical quantities, outputting via serial port using std::cout on mbed os 5

main.cpp

Committer:
skyscraper
Date:
2020-03-24
Revision:
2:9ffb2f18756b
Parent:
1:e11ab941748b
Child:
3:2834be4e10ef

File content as of revision 2:9ffb2f18756b:


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

namespace {
    
    DigitalOut led2(LED2);
    
    QUAN_QUANTITY_LITERAL(magnetic_flux_density,gauss);
    QUAN_QUANTITY_LITERAL(magnetic_flux_density,milli_gauss);
    QUAN_QUANTITY_LITERAL(magnetic_flux_density,uT);

    // terminal loop, printing message periodically
    void loop_forever(std::string const & str)
    {
        // stop but print error dynamically
        int count = 0;
        for (;;) {
            led2 = 1;
            std::cout << str << " " << count++ << '\n';
            ThisThread::sleep_for(200U);
            led2 = 0;
            ThisThread::sleep_for(800U);
        }
    }
}// namespace

int main()
{
    std::cout << "HMC5883 test\n";

    //wait for mag to init
    ThisThread::sleep_for(500U);

    bool success = mag_detected();
    if ( success ) {
        std::cout << "Detected a HMC5883\n";
    } else {
        loop_forever("Failed to detect HMC5883");
    }

    // N.b after offsets removed mag was reading around 33.6 uT, so not bad!
    constexpr auto earth_magnetic_field_flux_density = 31.869_uT;
    success =
        mag_set_samples_average(8) &&
        mag_set_data_rate<3,4>() &&
        // N.B if offsets are large then may need to set larger range
        // prob need to cycle through looking for best range
        // so this may not work
        mag_set_range( earth_magnetic_field_flux_density * 2U);
    if ( !success) {
        loop_forever("HMC5883 setup failed");
    }
    
    std::cout << " mag range set to " << mag_get_range() << '\n';

    // calculate the offsets dynamically by averaging 
    // the min and max over time
    quan::three_d::vect<quan::magnetic_flux_density::uT> voffsets;
    quan::three_d::vect<quan::magnetic_flux_density::uT> vmax;
    quan::three_d::vect<quan::magnetic_flux_density::uT> vmin;

    auto prev = Kernel::get_ms_count();
    for (;;) {

        quan::three_d::vect<quan::magnetic_flux_density::uT> values;
        if(mag_do_single_measurement(values)) {

            vmax.x = quan::max(vmax.x,values.x);
            vmax.y = quan::max(vmax.y,values.y);
            vmax.z = quan::max(vmax.z,values.z);

            vmin.x = quan::min(vmin.x,values.x);
            vmin.y = quan::min(vmin.y,values.y);
            vmin.z = quan::min(vmin.z,values.z);

            voffsets = (vmin + vmax)/2.f;

            values -= voffsets;

            std::cout << "val = " << values << '\n';
            std::cout << "off = " << voffsets << "\n\n";
        } else {
            std::cout << "mag read failed\n";
        }
        ThisThread::sleep_for(10U);
        auto now = Kernel::get_ms_count();
        if (( now - prev)> 500){
            prev = now;
            led2 = ! led2;
        }
    }
}