Line data    Source code

       1             : #include "SIM_MS5XXX.h"
       2             : 
       3             : #include <SITL/SITL.h>
       4             : 
       5             : #include <stdio.h>
       6             : 
       7             : using namespace SITL;
       8             : 
       9             : #include <AP_HAL/AP_HAL.h>
      10             : extern const AP_HAL::HAL& hal;
      11             : 
      12        4294 : MS5XXX::MS5XXX() :
      13        4294 :     I2CDevice()
      14             : {
      15        4294 : }
      16             : 
      17           4 : void MS5XXX::reset()
      18             : {
      19             :     // load prom from internal register:
      20           4 :     prom_loaded = true;
      21           4 :     load_prom(loaded_prom, sizeof(loaded_prom));
      22           4 : }
      23             : 
      24       64741 : void MS5XXX::convert_D1()
      25             : {
      26             :     float P_Pa;
      27             :     float temperature;
      28       64741 :     get_pressure_temperature_readings(P_Pa, temperature);
      29             : 
      30       64741 :     if (P_Pa < 0.1) {
      31             :         // maths breaks down on very, very low numbers, or there's a
      32             :         // bug in the conversion code.  The simulation can pass in
      33             :         // very, very low numbers.  Clamp it.
      34           0 :         P_Pa = 0.1;
      35             :     }
      36             : 
      37             :     uint32_t D1;
      38             :     uint32_t D2;
      39       64741 :     convert(P_Pa, temperature, D1, D2);
      40             : 
      41             :     // Check the accuracy of the returned conversion by utilizing a copy of the drivers
      42       64741 :     check_conversion_accuracy(P_Pa, temperature, D1, D2);
      43             : 
      44       64741 :     convert_out[2] = D1 & 0xff;
      45       64741 :     D1 >>= 8;
      46       64741 :     convert_out[1] = D1 & 0xff;
      47       64741 :     D1 >>= 8;
      48       64741 :     convert_out[0] = D1 & 0xff;
      49       64741 : }
      50       16191 : void MS5XXX::convert_D2()
      51             : {
      52             :     float P_Pa;
      53             :     float temperature;
      54       16191 :     get_pressure_temperature_readings(P_Pa, temperature);
      55             : 
      56       16191 :     if (P_Pa < 0.1) {
      57             :         // maths breaks down on very, very low numbers, or there's a
      58             :         // bug in the conversion code.  The simulation can pass in
      59             :         // very, very low numbers.  Clamp it.
      60           0 :         P_Pa = 0.1;
      61             : 
      62             :         // This should be a simulation error??? Pressure at 86km is 0.374Pa
      63             :     }
      64             : 
      65             :     uint32_t D1;
      66             :     uint32_t D2;
      67       16191 :     convert(P_Pa, temperature, D1, D2);
      68       16191 :     convert_out[2] = D2 & 0xff;
      69       16191 :     D2 >>= 8;
      70       16191 :     convert_out[1] = D2 & 0xff;
      71       16191 :     D2 >>= 8;
      72       16191 :     convert_out[0] = D2 & 0xff;
      73       16191 : }
      74             : 
      75             : 
      76   357519884 : void MS5XXX::update(const class Aircraft &aircraft)
      77             : {
      78   357519884 :     const uint32_t now_us = AP_HAL::micros();
      79             :     // static uint32_t then_us = 0;
      80             :     // ::fprintf(stderr, "update: s=%u now=%u delta=%u cmd-age=%u\n", (unsigned)state, (unsigned)now_us, (unsigned)(now_us - then_us), (unsigned)(now_us-command_start_us));
      81             :     // then_us = now_us;
      82             : 
      83   357519884 :     switch (state) {
      84        4018 :     case State::COLD:
      85        4018 :         command_start_us = now_us;
      86        4018 :         prom_loaded = false;
      87        4018 :         state = State::COLD_WAIT;
      88        4018 :         break;
      89        4018 :     case State::COLD_WAIT:
      90             :         // 1ms to do anything....
      91        4018 :         if (now_us - command_start_us < 1) {
      92           0 :             break;
      93             :         }
      94        4018 :         state = State::UNINITIALISED;
      95             :         FALLTHROUGH;
      96   356373890 :     case State::UNINITIALISED:
      97   356373890 :         break;
      98           4 :     case State::RESET_START:
      99           4 :         command_start_us = now_us;
     100           4 :         state = State::RESET_WAIT;
     101           4 :         break;
     102          12 :     case State::RESET_WAIT:
     103             :         // 2ms for reset to complete (data sheet does not specify?)
     104          12 :         if (now_us - command_start_us > 2000) {
     105           4 :             reset();
     106           4 :             state = State::RUNNING;
     107           4 :             break;
     108             :         }
     109           8 :         break;
     110             : 
     111       64741 :     case State::CONVERSION_D1_START:
     112       64741 :         command_start_us = now_us;
     113       64741 :         convert_out[0] = 0;
     114       64741 :         convert_out[1] = 0;
     115       64741 :         convert_out[2] = 0;
     116       64741 :         state = State::CONVERSION_D1_WAIT;
     117       64741 :         break;
     118      194223 :     case State::CONVERSION_D1_WAIT:
     119             :         // driver allows for 10ms for a conversion to happen
     120      194223 :         if (now_us - command_start_us > conversion_time_osr_1024_us) {
     121       64741 :             convert_D1();
     122       64741 :             state = State::RUNNING;
     123       64741 :             break;
     124             :         }
     125      129482 :         break;
     126             : 
     127       16191 :     case State::CONVERSION_D2_START:
     128       16191 :         command_start_us = now_us;
     129       16191 :         convert_out[0] = 0;
     130       16191 :         convert_out[1] = 0;
     131       16191 :         convert_out[2] = 0;
     132       16191 :         state = State::CONVERSION_D2_WAIT;
     133       16191 :         break;
     134       48573 :     case State::CONVERSION_D2_WAIT:
     135             :         // driver allows for 10ms for a conversion to happen
     136       48573 :         if (now_us - command_start_us > conversion_time_osr_1024_us) {
     137       16191 :             convert_D2();
     138       16191 :             state = State::RUNNING;
     139       16191 :             break;
     140             :         }
     141       32382 :         break;
     142             : 
     143      818232 :     case State::RUNNING:
     144      818232 :         break;
     145             :     }
     146             : 
     147             :     // float pressure = AP::sitl()->state.airspeed_raw_pressure[0];
     148             :     // float temperature = 25.0f;
     149   357519884 : }
     150             : 
     151      161896 : int MS5XXX::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
     152             : {
     153      161896 :     struct I2C::i2c_msg &msg = data->msgs[0];
     154             :     // if (data->nmsgs != 1) {
     155             :     //     AP_HAL::panic("nmsgs=%u", data->nmsgs);
     156             :     // }
     157      161896 :     if (msg.flags == I2C_M_RD) {
     158           0 :         AP_HAL::panic("Read (%u)",msg.len);
     159             :         return 0;
     160             :     }
     161             : 
     162      161896 :     if (msg.len != 1) {
     163           0 :         AP_HAL::panic("bad command length");
     164             :     }
     165      161896 :     const Command cmd = (Command)msg.buf[0];
     166      161896 :     if (state != State::RUNNING) {
     167           4 :         if (state == State::UNINITIALISED &&
     168             :             cmd == Command::RESET) {
     169             :             // this is OK - RESET is OK in UNINITIALISED
     170             :         } else {
     171           0 :             hal.console->printf("Command (0x%02x) received while not running (state=%u)\n", (unsigned)cmd, (unsigned)state);
     172           0 :             return -1;  // we could die instead...
     173             :         }
     174             :     }
     175             : 
     176      161896 :     switch (cmd) {
     177           4 :     case Command::RESET:
     178           4 :         state = State::RESET_START;
     179           4 :         break;
     180          32 :     case Command::READ_C0:
     181             :     case Command::READ_C1:
     182             :     case Command::READ_C2:
     183             :     case Command::READ_C3:
     184             :     case Command::READ_C4:
     185             :     case Command::READ_C5:
     186             :     case Command::READ_C6:
     187             :     case Command::READ_CRC: {
     188          32 :         if (data->msgs[1].len != 2) {
     189           0 :             AP_HAL::panic("Unexpected length");
     190             :         }
     191          32 :         const uint8_t addr = ((unsigned)cmd - (unsigned)Command::READ_C0)/2;
     192          32 :         const uint16_t val = htobe16(loaded_prom[addr]);
     193          32 :         data->msgs[1].buf[0] = val & 0xff;
     194          32 :         data->msgs[1].buf[1] = val >> 8;
     195          32 :         break;
     196             :     }
     197       64741 :     case Command::CONVERT_D1_OSR_1024:
     198       64741 :         state = State::CONVERSION_D1_START;
     199       64741 :         break;
     200       16191 :     case Command::CONVERT_D2_OSR_1024:
     201       16191 :         state = State::CONVERSION_D2_START;
     202       16191 :         break;
     203       80928 :     case Command::READ_CONVERSION:
     204       80928 :         if (data->msgs[1].len == 0) {
     205             :             // upon not getting a reading back the driver commands a
     206             :             // conversion-read but doesn't wait for a response!
     207           0 :             hal.console->printf("read of length zero\n");
     208           0 :             return -1;
     209             :         }
     210       80928 :         if (data->msgs[1].len != 3) {
     211           0 :             AP_HAL::panic("Unexpected length=%u", data->msgs[1].len);
     212             :         }
     213       80928 :         data->msgs[1].buf[0] = convert_out[0];
     214       80928 :         data->msgs[1].buf[1] = convert_out[1];
     215       80928 :         data->msgs[1].buf[2] = convert_out[2];
     216       80928 :         break;
     217           0 :     default:
     218           0 :         AP_HAL::panic("Unknown command %u (0x%02x)", (unsigned)cmd, (unsigned)cmd);
     219             :     }
     220      161896 :     return 0;
     221             : }