Diff: MS4525DO/MS4525DO.cpp

Revision:

0:5965bf423184

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MS4525DO/MS4525DO.cpp	Fri Sep 13 08:41:37 2019 +0000
@@ -0,0 +1,160 @@
+
+#include "mbed.h"
+#include "MS4525DO.h"
+
+
+MS4525DO::MS4525DO(PinName sda, PinName scl, char slave_adr)
+    :
+    i2c_p(new I2C(sda, scl)), 
+    i2c(*i2c_p),
+    address(slave_adr<<1)
+{
+    initialize();
+}
+
+MS4525DO::MS4525DO(I2C &i2c_obj, char slave_adr)
+    :
+    i2c_p(NULL), 
+    i2c(i2c_obj),
+    address(slave_adr<<1)
+{
+    initialize();
+}
+
+MS4525DO::~MS4525DO()
+{
+    if (NULL != i2c_p)
+        delete  i2c_p;
+}
+
+
+/* public functions
+        void initialize(void);
+        int measure(void);              // returns status of measurement
+        float getPSI(void);             // returns the PSI of last measurement
+        float getTemperature(void);     // returns temperature of last measurement
+        float getAirSpeed(void);        // calculates and returns the airspeed
+        int calibrate(void);            // attempts to calibrate and returns a status code
+        */
+
+void MS4525DO::initialize()
+{
+    // i2c writes to set up addresses, etc.
+    
+}
+
+// combine measure and collect into one function that returns the status code only, and holds the results in variables
+// other subroutines for returning clean values should be get functions
+
+
+int MS4525DO::measure()
+{
+    char ret;
+    ret = fetch_pressure(P_dat, T_dat);
+    return ret;
+}
+
+
+char MS4525DO::fetch_pressure(uint16_t &P_dat, uint16_t &T_dat)
+{
+    char _status;
+    char Press_H;
+    char Press_L;
+    char Temp_H;
+    char Temp_L;
+
+    char cmd[1];
+    cmd[0] = 0x00;
+    // So a device with a 'physical' address of 0x28, will need 'address bytes' of 0x50 for a write (R/W low), and 0x51 for a read (R/W high). 
+    //      MS4525DAddress
+    i2c.write(address, cmd, 1, true);  // select the register, no I2C Stop 
+       
+
+    char data[4];
+//    i2c.read(MS4525DAddress, static_cast<uint8_t>(4), static_cast<uint8_t>(true));  //Request 4 bytes, 2 pressure/status and 2 temperature
+    i2c.read(address, data, 4);
+    
+
+    Press_H = data[0];
+    Press_L = data[1];
+    Temp_H = data[2];
+    Temp_L = data[3];
+   
+     _status = (Press_H >> 6) & 0x03;
+    Press_H = Press_H & 0x3f;
+    P_dat = (((uint16_t)Press_H) << 8) | Press_L;
+
+    Temp_L = (Temp_L >> 5);
+    T_dat = (((uint16_t)Temp_H) << 3) | Temp_L;
+ 
+    return _status;
+}
+
+
+
+float MS4525DO::getPSI(void){             // returns the PSI of last measurement
+    // convert and store PSI
+    psi=(static_cast<float>(static_cast<int16_t>(P_dat)-MS4525ZeroCounts))/static_cast<float>(MS4525Span)*static_cast<float>(MS4525FullScaleRange);
+    // apply PSI calibration data
+    //   psi = psi + 0.007f;
+    
+    /* Below code is Pixhawk code which doesnt seem to work correctly */
+    // Calculate differential pressure. As its centered around 8000
+    // and can go positive or negative
+    /*
+    const float P_min = -1.0f;
+    const float P_max = 1.0f;
+    const float PSI_to_Pa = 6894.757f;
+    */ 
+    /*
+      this equation is an inversion of the equation in the
+      pressure transfer function figure on page 4 of the datasheet
+      We negate the result so that positive differential pressures
+      are generated when the bottom port is used as the static
+      port on the pitot and top port is used as the dynamic port
+     */
+     /*
+    float diff_press_PSI = -((T_dat - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min);
+    float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
+    */
+    
+    
+    return psi;
+}             
+  
+float MS4525DO::getTemperature(void){     // returns temperature of last measurement
+    // convert and store Temperature
+    temperature= (static_cast<float>(static_cast<int16_t>(T_dat)));
+    temperature = (temperature / 10);   // now in deg F
+    temperature = ((temperature -32) / 1.8f);   // now in deg C
+    
+    /* Below code is pixhawk version which DOES work correctly */
+    /*
+    PX4temperature = ((200.0f * T_dat) / 2047) - 50;
+    terminal.printf("PX4 Temperature: ");
+    terminal.printf("%0.1f\n", PX4temperature);
+    */
+    
+    return temperature;
+}
+
+float MS4525DO::getAirSpeed(void){        // calculates and returns the airspeed
+    /* Velocity calculation from a pitot tube explanation */
+    /* +/- 1PSI, approximately 100 m/s */
+    float rho = 1.225; // density of air 
+    // velocity = squareroot( (2*differential) / rho )
+    float velocity;
+    if (psi<0) {
+        velocity = -sqrt(-(2*psi) / rho);
+    }else{
+        velocity = sqrt((2*psi) / rho);
+        }
+    velocity = velocity*10;
+    
+    return velocity;
+}
+
+
+int MS4525DO::calibrate(void){            // attempts to calibrate and returns a status code
+
+}
\ No newline at end of file