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@9:87a7169b4d5c, 2020-03-26 (annotated)

Committer:
skyscraper
Date:
Thu Mar 26 21:35:19 2020 +0000
Revision:
9:87a7169b4d5c
Parent:
8:c5dc1ce10722
Child:
10:75c8ce89aeb7
make the interface simpler. Really need a mag service that send s via a queue etc. Also need to think about multiple mags

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