Andy Little / Mbed OS mag_test3

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

main.cpp@8:c5dc1ce10722, 2020-03-26 (annotated)

Committer:
skyscraper
Date:
Thu Mar 26 21:13:44 2020 +0000
Revision:
8:c5dc1ce10722
Parent:
6:cc08f2d64773
Child:
9:87a7169b4d5c
Try to make magnetometer more generic. Next to do a mag service and put in own thread

Who changed what in which revision?

UserRevisionLine numberNew contents of line
skyscraper 0:37dbfb036586 1
skyscraper 0:37dbfb036586 2 #include "mbed.h"
skyscraper 8:c5dc1ce10722 3 #include "magnetometer.h"
skyscraper 0:37dbfb036586 4 #include <quan/max.hpp>
skyscraper 0:37dbfb036586 5 #include <quan/min.hpp>
skyscraper 3:2834be4e10ef 6 #include <quan/time.hpp>
skyscraper 3:2834be4e10ef 7
skyscraper 4:53e468c49c17 8 void mag_adjusted_values(
skyscraper 4:53e468c49c17 9 quan::time::ms const & update_rate,
skyscraper 6:cc08f2d64773 10 quan::three_d::vect<double> const & gains,
skyscraper 6:cc08f2d64773 11 quan::three_d::vect<quan::magnetic_flux_density::uT> const & offsets
skyscraper 4:53e468c49c17 12 );
skyscraper 3:2834be4e10ef 13 void mag_raw_values(quan::time::ms const & update_rate);
skyscraper 3:2834be4e10ef 14 void mag_offset_calibrate();
skyscraper 0:37dbfb036586 15
skyscraper 2:9ffb2f18756b 16 namespace {
skyscraper 2:9ffb2f18756b 17
skyscraper 3:2834be4e10ef 18 QUAN_QUANTITY_LITERAL(time,ms);
skyscraper 0:37dbfb036586 19
skyscraper 2:9ffb2f18756b 20 // terminal loop, printing message periodically
skyscraper 2:9ffb2f18756b 21 void loop_forever(std::string const & str)
skyscraper 2:9ffb2f18756b 22 {
skyscraper 8:c5dc1ce10722 23 DigitalOut led1(LED1,1);
skyscraper 2:9ffb2f18756b 24 // stop but print error dynamically
skyscraper 2:9ffb2f18756b 25 int count = 0;
skyscraper 2:9ffb2f18756b 26 for (;;) {
skyscraper 8:c5dc1ce10722 27 led1 = 1;
skyscraper 2:9ffb2f18756b 28 std::cout << str << " " << count++ << '\n';
skyscraper 2:9ffb2f18756b 29 ThisThread::sleep_for(200U);
skyscraper 8:c5dc1ce10722 30 led1 = 0;
skyscraper 2:9ffb2f18756b 31 ThisThread::sleep_for(800U);
skyscraper 2:9ffb2f18756b 32 }
skyscraper 0:37dbfb036586 33 }
skyscraper 8:c5dc1ce10722 34
skyscraper 8:c5dc1ce10722 35 // do something with mag output
skyscraper 8:c5dc1ce10722 36 void callback(quan::three_d::vect<quan::magnetic_flux_density::uT> const & v)
skyscraper 8:c5dc1ce10722 37 {
skyscraper 8:c5dc1ce10722 38 std::cout << "val = " << v << '\n';
skyscraper 8:c5dc1ce10722 39 }
skyscraper 8:c5dc1ce10722 40
skyscraper 0:37dbfb036586 41 }// namespace
skyscraper 0:37dbfb036586 42
skyscraper 0:37dbfb036586 43 int main()
skyscraper 0:37dbfb036586 44 {
skyscraper 8:c5dc1ce10722 45 DigitalOut led2(LED2,1);
skyscraper 8:c5dc1ce10722 46 std::cout << "magnetometer test\n";
skyscraper 0:37dbfb036586 47
skyscraper 8:c5dc1ce10722 48 if ( !mag_init()){
skyscraper 8:c5dc1ce10722 49 loop_forever("failed to init magnetometer\n");
skyscraper 0:37dbfb036586 50 }
skyscraper 2:9ffb2f18756b 51
skyscraper 8:c5dc1ce10722 52 uint64_t constexpr update_rate_ms = 20U;
skyscraper 8:c5dc1ce10722 53
skyscraper 8:c5dc1ce10722 54 auto prev_led = Kernel::get_ms_count();
skyscraper 8:c5dc1ce10722 55 auto now = Kernel::get_ms_count();
skyscraper 8:c5dc1ce10722 56 // TODO Thread for this
skyscraper 8:c5dc1ce10722 57 for (;;){
skyscraper 8:c5dc1ce10722 58 mag_start_measurement();
skyscraper 8:c5dc1ce10722 59 do{
skyscraper 8:c5dc1ce10722 60 ThisThread::sleep_for(10U);
skyscraper 8:c5dc1ce10722 61 } while ( ! mag_data_ready() );
skyscraper 8:c5dc1ce10722 62
skyscraper 8:c5dc1ce10722 63 quan::three_d::vect<quan::magnetic_flux_density::uT> v;
skyscraper 8:c5dc1ce10722 64 if (mag_read(v)){
skyscraper 8:c5dc1ce10722 65 callback(v);
skyscraper 8:c5dc1ce10722 66 }
skyscraper 8:c5dc1ce10722 67 if ( (now - prev_led) >= 500U){
skyscraper 8:c5dc1ce10722 68 prev_led = now;
skyscraper 8:c5dc1ce10722 69 led2 = (led2 == 0) ? 1: 0;
skyscraper 8:c5dc1ce10722 70 }
skyscraper 8:c5dc1ce10722 71 auto const next_wake = now +
skyscraper 8:c5dc1ce10722 72 static_cast<uint64_t>(update_rate_ms);
skyscraper 8:c5dc1ce10722 73 ThisThread::sleep_until(next_wake);
skyscraper 8:c5dc1ce10722 74 now = next_wake;
skyscraper 8:c5dc1ce10722 75 }
skyscraper 0:37dbfb036586 76 }