firmware/src/modules/Telemetry/HealthTelemetry.cpp

#include "configuration.h"

#if !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR && !MESHTASTIC_EXCLUDE_HEALTH_TELEMETRY && !defined(ARCH_PORTDUINO)

#include "../mesh/generated/meshtastic/telemetry.pb.h"
#include "Default.h"
#include "HealthTelemetry.h"
#include "MeshService.h"
#include "NodeDB.h"
#include "Power.h"
#include "PowerFSM.h"
#include "Router.h"
#include "TransmitHistory.h"
#include "UnitConversions.h"
#include "gps/RTC.h"
#include "main.h"
#include "sleep.h"
#include "target_specific.h"
#include <OLEDDisplay.h>
#include <OLEDDisplayUi.h>

// Sensors
#include "Sensor/MAX30102Sensor.h"
#include "Sensor/MLX90614Sensor.h"

MAX30102Sensor max30102Sensor;
MLX90614Sensor mlx90614Sensor;

#define FAILED_STATE_SENSOR_READ_MULTIPLIER 10
#define DISPLAY_RECEIVEID_MEASUREMENTS_ON_SCREEN true

#if (HAS_SCREEN)
#include "graphics/ScreenFonts.h"
#endif
#include <Throttle.h>

static constexpr uint16_t TX_HISTORY_KEY_HEALTH_TELEMETRY = 0x8003;

int32_t HealthTelemetryModule::runOnce()
{
    if (sleepOnNextExecution == true) {
        if (shouldDeferDeepSleep())
            return PREFLIGHT_SLEEP_RETRY_MS;
        sleepOnNextExecution = false;
        uint32_t nightyNightMs = Default::getConfiguredOrDefaultMs(moduleConfig.telemetry.health_update_interval,
                                                                   default_telemetry_broadcast_interval_secs);
        LOG_DEBUG("Sleep for %ims, then awake to send metrics again", nightyNightMs);
        doDeepSleep(nightyNightMs, true, false);
    }

    uint32_t result = UINT32_MAX;

    if (!(moduleConfig.telemetry.health_measurement_enabled || moduleConfig.telemetry.health_screen_enabled)) {
        // If this module is not enabled, and the user doesn't want the display screen don't waste any OSThread time on it
        return disable();
    }

    if (firstTime) {
        // This is the first time the OSThread library has called this function, so do some setup
        firstTime = false;

        if (moduleConfig.telemetry.health_measurement_enabled) {
            LOG_INFO("Health Telemetry: init");
            // Initialize sensors
            if (mlx90614Sensor.hasSensor())
                result = mlx90614Sensor.runOnce();
            if (max30102Sensor.hasSensor())
                result = max30102Sensor.runOnce();
        }
        return result == UINT32_MAX ? disable() : setStartDelay();
    } else {
        // if we somehow got to a second run of this module with measurement disabled, then just wait forever
        if (!moduleConfig.telemetry.health_measurement_enabled) {
            return disable();
        }

        uint32_t lastTelemetry = transmitHistory ? transmitHistory->getLastSentToMeshMillis(TX_HISTORY_KEY_HEALTH_TELEMETRY) : 0;
        if (((lastTelemetry == 0) ||
             !Throttle::isWithinTimespanMs(lastTelemetry,
                                           Default::getConfiguredOrDefaultMsScaled(moduleConfig.telemetry.health_update_interval,
                                                                                   default_telemetry_broadcast_interval_secs,
                                                                                   numOnlineNodes, TrafficType::TELEMETRY))) &&
            airTime->isTxAllowedChannelUtil(config.device.role != meshtastic_Config_DeviceConfig_Role_SENSOR) &&
            airTime->isTxAllowedAirUtil()) {
            sendTelemetry();
            if (transmitHistory)
                transmitHistory->setLastSentToMesh(TX_HISTORY_KEY_HEALTH_TELEMETRY);
        } else if (((lastSentToPhone == 0) || !Throttle::isWithinTimespanMs(lastSentToPhone, sendToPhoneIntervalMs)) &&
                   (service->isToPhoneQueueEmpty())) {
            // Just send to phone when it's not our time to send to mesh yet
            // Only send while queue is empty (phone assumed connected)
            sendTelemetry(NODENUM_BROADCAST, true);
            lastSentToPhone = millis();
        }
    }
    if (sleepOnNextExecution) {
        // Honor the pre-sleep grace period armed in sendTelemetry(): OSThread reschedules with
        // this return value, which would otherwise override setIntervalFromNow() with the
        // sensor polling interval and trigger deep sleep while the TX is still on air
        return FIVE_SECONDS_MS;
    }
    return min(sendToPhoneIntervalMs, result);
}

bool HealthTelemetryModule::wantUIFrame()
{
    return moduleConfig.telemetry.health_screen_enabled;
}

void HealthTelemetryModule::drawFrame(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
{
    display->setTextAlignment(TEXT_ALIGN_LEFT);
    display->setFont(FONT_SMALL);

    if (lastMeasurementPacket == nullptr) {
        // If there's no valid packet, display "Health"
        display->drawString(x, y, "Health");
        display->drawString(x, y += _fontHeight(FONT_SMALL), "No measurement");
        return;
    }

    // Decode the last measurement packet
    meshtastic_Telemetry lastMeasurement;
    uint32_t agoSecs = service->GetTimeSinceMeshPacket(lastMeasurementPacket);
    const char *lastSender = getSenderShortName(*lastMeasurementPacket);

    const meshtastic_Data &p = lastMeasurementPacket->decoded;
    if (!pb_decode_from_bytes(p.payload.bytes, p.payload.size, &meshtastic_Telemetry_msg, &lastMeasurement)) {
        display->drawString(x, y, "Measurement Error");
        LOG_ERROR("Unable to decode last packet");
        return;
    }

    // Display "Health From: ..." on its own
    char agoStr[16];
    if (agoSecs == SINCE_UNKNOWN)
        snprintf(agoStr, sizeof(agoStr), "?"); // no trustworthy arrival time to age against
    else
        snprintf(agoStr, sizeof(agoStr), "%us", (unsigned)agoSecs);
    char headerStr[64];
    snprintf(headerStr, sizeof(headerStr), "Health From: %s(%s)", lastSender, agoStr);
    display->drawString(x, y, headerStr);

    char last_temp[16];
    if (moduleConfig.telemetry.environment_display_fahrenheit) {
        snprintf(last_temp, sizeof(last_temp), "%.0f°F",
                 UnitConversions::CelsiusToFahrenheit(lastMeasurement.variant.health_metrics.temperature));
    } else {
        snprintf(last_temp, sizeof(last_temp), "%.0f°C", lastMeasurement.variant.health_metrics.temperature);
    }

    // Continue with the remaining details
    char tempStr[32];
    snprintf(tempStr, sizeof(tempStr), "Temp: %s", last_temp);
    display->drawString(x, y += _fontHeight(FONT_SMALL), tempStr);
    if (lastMeasurement.variant.health_metrics.has_heart_bpm) {
        char heartStr[32];
        snprintf(heartStr, sizeof(heartStr), "Heart Rate: %u bpm", lastMeasurement.variant.health_metrics.heart_bpm);
        display->drawString(x, y += _fontHeight(FONT_SMALL), heartStr);
    }
    if (lastMeasurement.variant.health_metrics.has_spO2) {
        char spo2Str[32];
        snprintf(spo2Str, sizeof(spo2Str), "spO2: %u %%", lastMeasurement.variant.health_metrics.spO2);
        display->drawString(x, y += _fontHeight(FONT_SMALL), spo2Str);
    }
}

bool HealthTelemetryModule::handleReceivedProtobuf(const meshtastic_MeshPacket &mp, meshtastic_Telemetry *t)
{
    if (t->which_variant == meshtastic_Telemetry_health_metrics_tag) {
#if defined(DEBUG_PORT) && !defined(DEBUG_MUTE)
        const char *sender = getSenderShortName(mp);

        LOG_INFO("(Received from %s): temperature=%f, heart_bpm=%d, spO2=%d,", sender, t->variant.health_metrics.temperature,
                 t->variant.health_metrics.heart_bpm, t->variant.health_metrics.spO2);

#endif
        // release previous packet before occupying a new spot
        if (lastMeasurementPacket != nullptr)
            packetPool.release(lastMeasurementPacket);

        lastMeasurementPacket = packetPool.allocCopy(mp);
    }

    return false; // Let others look at this message also if they want
}

bool HealthTelemetryModule::getHealthTelemetry(meshtastic_Telemetry *m)
{
    bool valid = false;
    bool hasSensor = false;
    bool get_metrics;
    m->time = getTime();
    m->which_variant = meshtastic_Telemetry_health_metrics_tag;
    m->variant.health_metrics = meshtastic_HealthMetrics_init_zero;

    if (max30102Sensor.hasSensor()) {
        get_metrics = max30102Sensor.getMetrics(m);
        valid = valid || get_metrics; // avoid short-circuit evaluation rules
        hasSensor = true;
    }
    if (mlx90614Sensor.hasSensor()) {
        get_metrics = mlx90614Sensor.getMetrics(m);
        valid = valid || get_metrics;
        hasSensor = true;
    }

    return valid && hasSensor;
}

meshtastic_MeshPacket *HealthTelemetryModule::allocReply()
{
    if (currentRequest) {
        if (isMultiHopBroadcastRequest() && !isSensorOrRouterRole()) {
            ignoreRequest = true;
            return NULL;
        }
        auto req = *currentRequest;
        const auto &p = req.decoded;
        meshtastic_Telemetry scratch;
        meshtastic_Telemetry *decoded = NULL;
        memset(&scratch, 0, sizeof(scratch));
        if (pb_decode_from_bytes(p.payload.bytes, p.payload.size, &meshtastic_Telemetry_msg, &scratch)) {
            decoded = &scratch;
        } else {
            LOG_ERROR("Error decoding HealthTelemetry module!");
            return NULL;
        }
        // Check for a request for health metrics
        if (decoded->which_variant == meshtastic_Telemetry_health_metrics_tag) {
            meshtastic_Telemetry m = meshtastic_Telemetry_init_zero;
            if (getHealthTelemetry(&m)) {
                LOG_INFO("Health telemetry reply to request");
                return allocDataProtobuf(m);
            } else {
                return NULL;
            }
        }
    }
    return NULL;
}

bool HealthTelemetryModule::sendTelemetry(NodeNum dest, bool phoneOnly)
{
    meshtastic_Telemetry m = meshtastic_Telemetry_init_zero;
    m.which_variant = meshtastic_Telemetry_health_metrics_tag;
    m.time = getTime();
    bool validTelemetry = getHealthTelemetry(&m);
    if (validTelemetry) {
        LOG_INFO("Send: temperature=%f, heart_bpm=%d, spO2=%d", m.variant.health_metrics.temperature,
                 m.variant.health_metrics.heart_bpm, m.variant.health_metrics.spO2);

        sensor_read_error_count = 0;

        meshtastic_MeshPacket *p = allocDataProtobuf(m);
        if (!p) {
            validTelemetry = false;
        } else {
            p->to = dest;
            p->decoded.want_response = false;
            if (config.device.role == meshtastic_Config_DeviceConfig_Role_SENSOR)
                p->priority = meshtastic_MeshPacket_Priority_RELIABLE;
            else
                p->priority = meshtastic_MeshPacket_Priority_BACKGROUND;
            // release previous packet before occupying a new spot
            if (lastMeasurementPacket != nullptr)
                packetPool.release(lastMeasurementPacket);

            lastMeasurementPacket = packetPool.allocCopy(*p);
            if (phoneOnly) {
                LOG_INFO("Send packet to phone");
                service->sendToPhone(p);
            } else {
                LOG_INFO("Send packet to mesh");
                service->sendToMesh(p, RX_SRC_LOCAL, true);
            }
        }
    }

    // Arm the pre-sleep sequence even when no valid reading was available this cycle: a
    // power-saving SENSOR node must still return to deep sleep, otherwise it stays awake
    // until the next telemetry interval and drains its battery
    if (!phoneOnly && isPowerSavingSensor()) {
        if (!validTelemetry)
            LOG_WARN("Health telemetry unavailable this cycle, sleep without sending");
        sleepOnNextExecution = true;
        preflightSleepDeferrals = 0;
        LOG_DEBUG("Start next execution in 5s, then sleep");
        setIntervalFromNow(FIVE_SECONDS_MS);
    }
    return validTelemetry;
}

#endif