Add Power and Energy telemetry if Voltage and Current analog sensors are both available

board/project/common.c

@@ -29,6 +29,19 @@ float get_consumption(float current, uint16_t current_max, uint32_t *timestamp)
     return mah;
 }
 
+float get_energy(float power, uint16_t power_max, uint32_t *timestamp_power) {
+    if (!*timestamp_power) {
+        *timestamp_power = time_us_32();
+        return 0;
+    }
+    uint32_t now = time_us_32();                    // us
+    uint32_t interval = (now - *timestamp_power) / 1000;  // ms
+    float Wh = power * interval / 3600000.0;
+    *timestamp_power = now;
+    if (interval > 2000 || (power_max && (Wh > power_max * interval / 3600000.0))) return 0;
+    return Wh;
+}
+
 float voltage_read(uint8_t adc_num) {
     adc_select_input(adc_num);
     return adc_read() * BOARD_VCC / ADC_RESOLUTION;

board/project/common.h

@@ -54,6 +54,7 @@ typedef struct context_t {
 
 float get_average(float alpha, float prev_value, float new_value);
 float get_consumption(float current, uint16_t current_max, uint32_t *timestamp);
+float get_energy(float power, uint16_t power_max, uint32_t *timestamp_power);
 float voltage_read(uint8_t adc_num);
 float get_altitude(float pressure, float temperature, float P0);
 void get_vspeed(float *vspeed, float altitude, uint interval);

board/project/constants.h

@@ -91,6 +91,7 @@
 
 #define STACK_VOLTAGE (156 + STACK_EXTRA)
 #define STACK_CURRENT (166 + STACK_EXTRA)
+#define STACK_POWER (360 + STACK_EXTRA)
 #define STACK_NTC (160 + STACK_EXTRA)
 #define STACK_AIRSPEED (184 + STACK_EXTRA)
 #define STACK_FUEL_METER (200 + STACK_EXTRA)

board/project/protocol/jetiex.c

@@ -8,6 +8,7 @@
 #include "bmp280.h"
 #include "config.h"
 #include "current.h"
+#include "power.h"
 #include "esc_apd_f.h"
 #include "esc_apd_hv.h"
 #include "esc_castle.h"
@@ -902,7 +903,7 @@ static void set_config(sensor_jetiex_t **sensor) {
         add_sensor(new_sensor, sensor);
         ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
     }
-    if (config->enable_analog_voltage) {
+    if (config->enable_analog_voltage && !config->enable_analog_current) {
         voltage_parameters_t parameter = {0, config->analog_rate, config->alpha_voltage,
                                           config->analog_voltage_multiplier, malloc(sizeof(float))};
         xTaskCreate(voltage_task, "voltage_task", STACK_VOLTAGE, (void *)&parameter, 2, &task_handle);
@@ -913,7 +914,7 @@ static void set_config(sensor_jetiex_t **sensor) {
         add_sensor(new_sensor, sensor);
         ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
     }
-    if (config->enable_analog_current) {
+    if (config->enable_analog_current && !config->enable_analog_voltage) {
         current_parameters_t parameter = {1,
                                           config->analog_rate,
                                           config->alpha_current,
@@ -934,6 +935,49 @@ static void set_config(sensor_jetiex_t **sensor) {
         add_sensor(new_sensor, sensor);
         ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
     }
+    if (config->enable_analog_current && config->enable_analog_voltage) {
+        power_parameters_t parameter = {1,
+                                        0,
+                                        config->analog_rate,
+                                        config->alpha_current,
+                                        config->analog_current_multiplier,
+                                        config->analog_voltage_multiplier,
+                                        config->analog_current_offset,
+                                        config->analog_current_autoffset,
+                                        malloc(sizeof(float)),
+                                        malloc(sizeof(float)),
+                                        malloc(sizeof(float)),
+                                        malloc(sizeof(float)),
+                                        malloc(sizeof(float)),
+                                        malloc(sizeof(float))};
+        xTaskCreate(power_task, "power_task", STACK_POWER, (void *)&parameter, 2, &task_handle);
+        xQueueSendToBack(context.tasks_queue_handle, task_handle, 0);
+
+        new_sensor = malloc(sizeof(sensor_jetiex_t));
+        *new_sensor =
+            (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_2_DECIMAL, "Voltage", "V", parameter.voltage};
+        add_sensor(new_sensor, sensor);
+
+        new_sensor = malloc(sizeof(sensor_jetiex_t));
+        *new_sensor =
+            (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_1_DECIMAL, "Current", "A", parameter.current};
+        add_sensor(new_sensor, sensor);
+
+        new_sensor = malloc(sizeof(sensor_jetiex_t));
+        *new_sensor =
+            (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "Power", "W", parameter.power};
+        add_sensor(new_sensor, sensor);
+
+        new_sensor = malloc(sizeof(sensor_jetiex_t));
+        *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT22, JETIEX_FORMAT_0_DECIMAL, "Consumption", "mAh", parameter.consumption};
+        add_sensor(new_sensor, sensor);
+
+        new_sensor = malloc(sizeof(sensor_jetiex_t));
+        *new_sensor = (sensor_jetiex_t){0, JETIEX_TYPE_INT14, JETIEX_FORMAT_0_DECIMAL, "Energy", "Wh", parameter.power_consumption};
+        add_sensor(new_sensor, sensor);        
+
+        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+    }
     if (config->enable_analog_ntc) {
         ntc_parameters_t parameter = {2, config->analog_rate, config->alpha_temperature, malloc(sizeof(float))};
         xTaskCreate(ntc_task, "ntc_task", STACK_NTC, (void *)&parameter, 2, &task_handle);

board/project/sensor/CMakeLists.txt

@@ -26,4 +26,5 @@ target_sources(${PROJECT_NAME} PRIVATE
     smart_esc.c
     esc_omp_m4.c
     esc_ztw.c
+    power.c
 )

board/project/sensor/power.c

@@ -0,0 +1,60 @@
+
+#include "power.h"
+
+#include <stdio.h>
+
+#include "auto_offset.h"
+#include "hardware/adc.h"
+#include "pico/stdlib.h"
+
+void power_task(void *parameters) {
+    static uint32_t timestamp = 0;
+    static uint32_t timestamp_power = 0;
+    power_parameters_t parameter = *(power_parameters_t *)parameters;
+    *parameter.voltage_current = 0;
+    *parameter.voltage = 0;
+    *parameter.current = 0;
+    *parameter.consumption = 0;
+    *parameter.power = 0;
+    *parameter.power_consumption = 0;
+    xTaskNotifyGive(context.receiver_task_handle);
+    adc_init();
+    adc_gpio_init(parameter.adc_current_num + 26);
+    adc_gpio_init(parameter.adc_voltage_num + 26);
+    if (parameter.auto_offset) {
+        parameter.offset = -1;
+        auto_offset_float_parameters_t parameter_auto_offset = {5000, parameter.voltage_current, &parameter.offset};
+        TaskHandle_t task_handle;
+        xTaskCreate(auto_offset_float_task, "analog_current_auto_offset_task", STACK_AUTO_OFFSET,
+                    &parameter_auto_offset, 2, &task_handle);
+    }
+
+    while (1) {
+        *parameter.voltage_current = voltage_read(parameter.adc_current_num);
+        if (parameter.offset != -1)
+            *parameter.current = get_average(parameter.alpha, *parameter.current,
+                                             (*parameter.voltage_current - parameter.offset) * parameter.multiplier_current);
+
+        *parameter.voltage = get_average(parameter.alpha, *parameter.voltage, voltage_read(parameter.adc_voltage_num) * parameter.multiplier_voltage);
+        *parameter.power = *parameter.voltage * *parameter.current;
+        if (time_us_32() > 6000000) {
+            *parameter.consumption += get_consumption(*parameter.current, 0, &timestamp);
+            *parameter.power_consumption += get_energy(*parameter.power, 0, &timestamp_power);
+        }
+
+
+#ifdef SIM_SENSORS
+        *parameter.voltage = 15.53;       
+        *parameter.current = 12.34;
+        *parameter.voltage_current = 2.56;
+        *parameter.consumption = 120.34;
+        *parameter.power = 70.14;
+        *parameter.power_consumption = 40;
+
+#endif
+        debug("\nCurrent (%u): Curr %.2f Cons %.2f Volt_current %.2f VoltOffs %.2f Multip_curr %.2f",
+              uxTaskGetStackHighWaterMark(NULL), *parameter.current, *parameter.consumption, *parameter.voltage_current,
+              parameter.offset, parameter.multiplier_current);
+        vTaskDelay(1000 / parameter.rate / portTICK_PERIOD_MS);
+    }
+}

board/project/sensor/power.h

@@ -0,0 +1,19 @@
+#ifndef POWER_H
+#define POWER_H
+
+#include "common.h"
+
+typedef struct power_parameters_t {
+    uint8_t adc_current_num;
+    uint8_t adc_voltage_num;
+    uint8_t rate;
+    float alpha, multiplier_current, multiplier_voltage, offset;
+    bool auto_offset;
+    float *current, *consumption, *voltage_current, *voltage, *power, *power_consumption;
+} power_parameters_t;
+
+extern context_t context;
+
+void power_task(void *parameters);
+
+#endif