Merge changes Iccc92d99,I473d7839,Iff5c2071

* changes:
  firmware: Add new type SENS_TYPE_LEDS_I2C for LED driver LP3943
  nanohub: drivers/st_lps22hb: added sensorCfgData and Calibration functions
  nanotool: accept events from STMicroelectronics lps22hb sensor driver

5 files changed, 110 insertions, 3 deletions

diff --git a/firmware/os/drivers/leds/leds_lp3943.c b/firmware/os/drivers/leds/leds_lp3943.c
index d05571ff..c1dbeb9f 100644
--- a/firmware/os/drivers/leds/leds_lp3943.c
+++ b/firmware/os/drivers/leds/leds_lp3943.c
@@ -250,7 +250,7 @@ static void sensorLedsOnOff(bool flag)
 
 static const struct SensorInfo sensorInfoLedsLP3943 = {
     .sensorName = "Leds-LP3943",
-    .sensorType = SENS_TYPE_LEDS,
+    .sensorType = SENS_TYPE_LEDS_I2C,
     .supportedRates = ledsRates,
 };
 
diff --git a/firmware/os/drivers/st_lps22hb/lps22hb.c b/firmware/os/drivers/st_lps22hb/lps22hb.c
index 0176f513..854256df 100644
--- a/firmware/os/drivers/st_lps22hb/lps22hb.c
+++ b/firmware/os/drivers/st_lps22hb/lps22hb.c
@@ -21,6 +21,7 @@
 #include <plat/gpio.h>
 #include <platform.h>
 #include <plat/syscfg.h>
+#include <heap.h>
 #include <sensors.h>
 #include <seos.h>
 #include <slab.h>
@@ -52,6 +53,9 @@
 #define LPS22HB_ODR_50_HZ               0x40
 #define LPS22HB_ODR_75_HZ               0x50
 
+#define LPS22HB_RPDS_L                  0x18
+#define LPS22HB_RPDS_H                  0x19
+
 #define LPS22HB_PRESS_OUTXL_REG_ADDR    0x28
 #define LPS22HB_TEMP_OUTL_REG_ADDR      0x2B
 
@@ -93,6 +97,11 @@ enum lps22hbSensorState {
     SENSOR_VERIFY_ID,
     SENSOR_BARO_POWER_UP,
     SENSOR_BARO_POWER_DOWN,
+    SENSOR_BARO_START_CAL,
+    SENSOR_BARO_READ_CAL_MEAS,
+    SENSOR_BARO_CAL_DONE,
+    SENSOR_BARO_SET_OFFSET,
+    SENSOR_BARO_CFG_DONE,
     SENSOR_TEMP_POWER_UP,
     SENSOR_TEMP_POWER_DOWN,
     SENSOR_READ_SAMPLES,
@@ -122,6 +131,12 @@ struct lps22hbSensor {
     uint32_t handle;
 };
 
+struct CalibrationData {
+    struct HostHubRawPacket header;
+    struct SensorAppEventHeader data_header;
+    float value;
+} __attribute__((packed));
+
 #define LPS22HB_MAX_PENDING_I2C_REQUESTS   4
 #define LPS22HB_MAX_I2C_TRANSFER_SIZE      6
 #define LPS22HB_MAX_BARO_EVENTS            4
@@ -154,6 +169,9 @@ struct lps22hbTask {
     bool tempReading;
     bool tempWantRead;
 
+    uint8_t offset_L;
+    uint8_t offset_H;
+
     //int sensLastRead;
 
     struct I2cTransfer transfers[LPS22HB_MAX_PENDING_I2C_REQUESTS];
@@ -262,6 +280,26 @@ static bool i2c_write(uint8_t addr, uint8_t data, uint32_t delay, uint8_t state)
     return (ret == -1) ? false : true;
 }
 
+static void sendCalibrationResult(uint8_t status, float value)
+{
+    struct CalibrationData *data = heapAlloc(sizeof(struct CalibrationData));
+    if (!data) {
+        ERROR_PRINT("Couldn't alloc cal result pkt\n");
+        return;
+    }
+
+    data->header.appId = LPS22HB_APP_ID;
+    data->header.dataLen = (sizeof(struct CalibrationData) - sizeof(struct HostHubRawPacket));
+    data->data_header.msgId = SENSOR_APP_MSG_ID_CAL_RESULT;
+    data->data_header.sensorType = SENS_TYPE_BARO;
+    data->data_header.status = status;
+
+    data->value = value;
+
+    if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree))
+        ERROR_PRINT("Couldn't send cal result evt\n");
+}
+
 /* Sensor Info */
 static void sensorBaroTimerCallback(uint32_t timerId, void *data)
 {
@@ -367,6 +405,46 @@ static bool baroFlush(void *cookie)
     return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_BARO), SENSOR_DATA_EVENT_FLUSH, NULL);
 }
 
+static bool baroCalibrate(void *cookie)
+{
+    INFO_PRINT("baroCalibrate\n");
+
+    if (mTask.baroOn) {
+        ERROR_PRINT("cannot calibrate while baro is active\n");
+        sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, 0.0f);
+        return false;
+    }
+
+    mTask.comm_tx(LPS22HB_RPDS_L, 0, 0, SENSOR_BARO_START_CAL);
+    return true;
+}
+
+/*
+ * Offset data is sent in hPa, and must be transformed in 16th of hPa.
+ * Since offset is expected to be summed to the out regs but the sensor
+ * will actually subctract it then we need to invert the sign.
+ */
+static bool baroCfgData(void *data, void *cookie)
+{
+    float offset_f = *((float *)data) * 16;
+    int32_t offset;
+    bool ret;
+
+    offset_f = (offset_f > 0) ? offset_f + 0.5f : offset_f - 0.5f;
+    offset = -(int32_t)offset_f;
+
+    INFO_PRINT("baroCfgData %ld\n", offset);
+
+    mTask.offset_H = (offset >> 8) & 0xff;
+    mTask.offset_L = (offset & 0xff);
+
+    ret = mTask.comm_tx(LPS22HB_RPDS_L, mTask.offset_L, 0, SENSOR_BARO_SET_OFFSET);
+    if (!ret)
+        DEBUG_PRINT("baroCfgData: comm_tx failed\n");
+
+    return ret;
+}
+
 static bool tempPower(bool on, void *cookie)
 {
     bool oldMode = mTask.baroOn || mTask.tempOn;
@@ -433,7 +511,7 @@ static bool tempFlush(void *cookie)
 
 static const struct SensorOps lps22hbSensorOps[NUM_OF_SENSOR] =
 {
-    { DEC_OPS(baroPower, baroFwUpload, baroSetRate, baroFlush, NULL, NULL) },
+    { DEC_OPS(baroPower, baroFwUpload, baroSetRate, baroFlush, baroCalibrate, baroCfgData) },
     { DEC_OPS(tempPower, tempFwUpload, tempSetRate, tempFlush, NULL, NULL) },
 };
 
@@ -492,6 +570,30 @@ static int handleCommDoneEvt(const void* evtData)
                     SENSOR_INTERNAL_EVT_POWER_STATE_CHG, false, 0);
         break;
 
+    case SENSOR_BARO_START_CAL:
+        mTask.comm_tx(LPS22HB_RPDS_H, 0, 0, SENSOR_BARO_READ_CAL_MEAS);
+        break;
+
+    case SENSOR_BARO_READ_CAL_MEAS:
+        mTask.comm_rx(LPS22HB_PRESS_OUTXL_REG_ADDR, 3, 1, SENSOR_BARO_CAL_DONE);
+        break;
+
+    case SENSOR_BARO_CAL_DONE:
+        ptr_samples = xfer->txrxBuf;
+
+        baro_val = ((ptr_samples[2] << 16) & 0xff0000) |
+                   ((ptr_samples[1] << 8) & 0xff00) | (ptr_samples[0]);
+
+        sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS, LPS22HB_HECTO_PASCAL((float)baro_val));
+        break;
+
+    case SENSOR_BARO_SET_OFFSET:
+        mTask.comm_tx(LPS22HB_RPDS_H, mTask.offset_H, 0, SENSOR_BARO_CFG_DONE);
+        break;
+
+    case SENSOR_BARO_CFG_DONE:
+        break;
+
     case SENSOR_READ_SAMPLES:
         if (mTask.baroOn && mTask.baroWantRead) {
             float pressure_hPa;
@@ -603,6 +705,9 @@ static bool startTask(uint32_t task_id)
     mTask.baroOn = mTask.tempOn = false;
     mTask.baroReading = mTask.tempReading = false;
 
+    mTask.offset_H = 0;
+    mTask.offset_L = 0;
+
     slabSize = sizeof(struct SingleAxisDataEvent) + sizeof(struct SingleAxisDataPoint);
 
     mTask.baroSlab = slabAllocatorNew(slabSize, 4, LPS22HB_MAX_BARO_EVENTS);
diff --git a/firmware/os/inc/sensType.h b/firmware/os/inc/sensType.h
index 9fa2bd12..9f32f2b7 100644
--- a/firmware/os/inc/sensType.h
+++ b/firmware/os/inc/sensType.h
@@ -81,6 +81,7 @@ extern "C" {
 #define SENS_TYPE_MAG_RAW         59
 #define SENS_TYPE_LEDS            60
 #define SENS_TYPE_HUMIDITY        61
+#define SENS_TYPE_LEDS_I2C        62
 
 #define SENS_TYPE_FIRST_USER      64 // event type necessarily begins with UserSensorEventHdr
 #define SENS_TYPE_LAST_USER       128
diff --git a/util/nanotool/apptohostevent.cpp b/util/nanotool/apptohostevent.cpp
index a94420b3..d7ef5757 100644
--- a/util/nanotool/apptohostevent.cpp
+++ b/util/nanotool/apptohostevent.cpp
@@ -72,7 +72,7 @@ bool AppToHostEvent::CheckAppId(SensorType sensor_type) const {
         break;
 
       case SensorType::Barometer:
-        if (GetAppId() != kAppIdBoschBmp280) {
+        if (GetAppId() != kAppIdBoschBmp280 && GetAppId() != kAppIdSTMicroLps22hb) {
             return false;
         }
         break;
diff --git a/util/nanotool/apptohostevent.h b/util/nanotool/apptohostevent.h
index 0a5ef54a..54c61b67 100644
--- a/util/nanotool/apptohostevent.h
+++ b/util/nanotool/apptohostevent.h
@@ -66,6 +66,7 @@ constexpr uint64_t kAppIdAmsTmd2772        = MakeAppId(kAppIdVendorGoogle, 9);
 constexpr uint64_t kAppIdRohmRpr0521       = MakeAppId(kAppIdVendorGoogle, 10);
 constexpr uint64_t kAppIdAmsTmd4903        = MakeAppId(kAppIdVendorGoogle, 12);
 constexpr uint64_t kAppIdSTMicroLsm6dsm    = MakeAppId(kAppIdVendorSTMicro, 0);
+constexpr uint64_t kAppIdSTMicroLps22hb    = MakeAppId(kAppIdVendorSTMicro, 1);
 constexpr uint64_t kAppIdSTMicroMag40      = MakeAppId(kAppIdVendorSTMicro, 3);
 
 constexpr uint64_t kAppIdBridge = MakeAppId(kAppIdVendorGoogle, 50);