diff --git a/omi/firmware/omi/src/battery.c b/omi/firmware/omi/src/battery.c index ee0a86693db..f34ff954a31 100644 --- a/omi/firmware/omi/src/battery.c +++ b/omi/firmware/omi/src/battery.c @@ -23,6 +23,14 @@ int16_t sample_buffer[ADC_TOTAL_SAMPLES + 1]; #define ADC_ACQUISITION_TIME ADC_ACQ_TIME(ADC_ACQ_TIME_MICROSECONDS, 10) #define ADC_1ST_CHANNEL_ID 0 #define ADC_1ST_CHANNEL_INPUT NRF_SAADC_INPUT_AIN0 +#define BATTERY_FILTER_ALPHA_U16 (uint16_t)(65535/(5+1)) +#define FILTER_INIT_CYCLES 5 +#define BATTERY_STATES(is_charging) ((is_charging) ? battery_charging_states : battery_discharge_states) + +// Static variable to store previous EMA value for battery percentage +static uint8_t battery_percentage_ema = 0; +static bool ema_initialized = false; +static uint8_t ema_init_counter = 0; static const struct device *const adc_dev = DEVICE_DT_GET(DT_NODELABEL(adc)); static const struct gpio_dt_spec power_pin = GPIO_DT_SPEC_GET_OR(DT_NODELABEL(power_pin), gpios, {0}); @@ -39,18 +47,33 @@ typedef struct { uint8_t percentage; } BatteryState; -BatteryState battery_states[BATTERY_STATES_COUNT] = { +BatteryState battery_discharge_states[BATTERY_STATES_COUNT] = { + {4140, 100}, + {4135, 99}, + {4091, 91}, + {4020, 78}, + {3938, 63}, + {3884, 53}, + {3791, 36}, + {3785, 35}, + {3671, 14}, + {3655, 11}, + {3600, 1}, // Threshold for <1% + {0000, 0} // Below safe level +}; + +BatteryState battery_charging_states[BATTERY_STATES_COUNT] = { {4200, 100}, - {4160, 99}, - {4090, 91}, - {4030, 78}, - {3890, 63}, - {3830, 53}, - {3680, 36}, - {3660, 35}, - {3480, 14}, - {3420, 11}, - {3400, 1}, // Threshold for <1% + {4195, 99}, + {4159, 91}, + {4100, 78}, + {4032, 63}, + {3986, 53}, + {3909, 36}, + {3905, 35}, + {3809, 14}, + {3795, 11}, + {3750, 1}, // Threshold for <1% {0000, 0} // Below safe level }; @@ -87,6 +110,29 @@ struct adc_sequence sequence = { .resolution = ADC_RESOLUTION, }; +uint8_t update_ema_filter(uint32_t current_ema, uint8_t new_value) +{ + // handle edge case transitions directly + if ((!is_charging && (current_ema <= 5)) || (is_charging && (current_ema >= 95))) { + if (is_charging) { + return (new_value > current_ema) ? current_ema + 1 : current_ema; + } else { + return (new_value < current_ema) ? current_ema - 1 : current_ema; + } + } + + // Constant coefficient Alpha for EMA calculation, scaled to 16 bit. + // Alpha = 65535/(N+1) where N is the averaging window + const uint32_t alpha = BATTERY_FILTER_ALPHA_U16; + const uint32_t alpha_complement = UINT16_MAX - BATTERY_FILTER_ALPHA_U16; + + // Calculate new EMA: new_ema = (alpha * new_value + alpha_complement * current_ema) / 65535 + uint64_t new_ema = (alpha * new_value) + (alpha_complement * current_ema); + + // Scale result back to 8-bit, with rounding up + return (uint8_t)((new_ema + 32768) >> 16); +} + static void battery_charging_callback(const struct device *dev, struct gpio_callback *cb, uint32_t pins) { int err = battery_charging_state_read(); @@ -227,29 +273,54 @@ int battery_get_millivolt(uint16_t *battery_millivolt) int battery_get_percentage(uint8_t *battery_percentage, uint16_t battery_millivolt) { + uint8_t raw_percentage = 0; + BatteryState *battery_states = BATTERY_STATES(is_charging); + // Use the battery discharge profile to determine percentage if (battery_millivolt >= battery_states[0].millivolts) { - *battery_percentage = battery_states[0].percentage; - return 0; + raw_percentage = battery_states[0].percentage; + } else if (battery_millivolt <= battery_states[BATTERY_STATES_COUNT - 1].millivolts) { + raw_percentage = battery_states[BATTERY_STATES_COUNT - 1].percentage; + } else { + // Find the appropriate range in the battery profile + for (int i = 0; i < BATTERY_STATES_COUNT - 1; i++) { + if (battery_millivolt <= battery_states[i].millivolts && battery_millivolt > battery_states[i + 1].millivolts) { + + // Linear interpolation between the two closest points + uint16_t voltage_range = battery_states[i].millivolts - battery_states[i + 1].millivolts; + uint8_t percentage_range = battery_states[i].percentage - battery_states[i + 1].percentage; + uint16_t voltage_diff = battery_states[i].millivolts - battery_millivolt; + + raw_percentage = battery_states[i].percentage - (voltage_diff * percentage_range) / voltage_range; + break; + } + } } - if (battery_millivolt <= battery_states[BATTERY_STATES_COUNT - 1].millivolts) { - *battery_percentage = battery_states[BATTERY_STATES_COUNT - 1].percentage; - return 0; + // Prevent sudden jumps in percentage + if (battery_percentage_ema != 0) { + if (is_charging && raw_percentage < battery_percentage_ema) { + raw_percentage = battery_percentage_ema; + } else if (!is_charging && raw_percentage > battery_percentage_ema) { + raw_percentage = battery_percentage_ema; + } } - // Find the appropriate range in the battery profile - for (int i = 0; i < BATTERY_STATES_COUNT - 1; i++) { - if (battery_millivolt <= battery_states[i].millivolts && battery_millivolt > battery_states[i + 1].millivolts) { - - // Linear interpolation between the two closest points - uint16_t voltage_range = battery_states[i].millivolts - battery_states[i + 1].millivolts; - uint8_t percentage_range = battery_states[i].percentage - battery_states[i + 1].percentage; - uint16_t voltage_diff = battery_states[i].millivolts - battery_millivolt; - - *battery_percentage = battery_states[i].percentage - (voltage_diff * percentage_range) / voltage_range; - break; + // Initialize EMA with first reading + if (!ema_initialized) { + battery_percentage_ema = raw_percentage; + ema_init_counter++; + + // Run filter for FILTER_INIT_CYCLES to stabilize + if (ema_init_counter >= FILTER_INIT_CYCLES) { + ema_initialized = true; } + + *battery_percentage = raw_percentage; + } else { + // Apply EMA filter to smooth out percentage changes + battery_percentage_ema = update_ema_filter(battery_percentage_ema, raw_percentage); + *battery_percentage = battery_percentage_ema; } return 0; diff --git a/omi/firmware/omi/src/lib/core/button.c b/omi/firmware/omi/src/lib/core/button.c index 03fb43aaeac..7ac1041a403 100644 --- a/omi/firmware/omi/src/lib/core/button.c +++ b/omi/firmware/omi/src/lib/core/button.c @@ -219,23 +219,6 @@ void check_button_level(struct k_work *work_item) btn_last_event = event; notify_tap(); - // Immediate feedback: LED off and haptic - led_off(); - // Set is_off immediately so set_led_state() keeps LEDs off - is_off = true; - -#ifdef CONFIG_OMI_ENABLE_HAPTIC - play_haptic_milli(100); - k_msleep(300); - haptic_off(); -#endif - - // Delays for stability - k_msleep(1000); - - // // Enter the low power mode - transport_off(); - k_msleep(300); turnoff_all(); } @@ -365,6 +348,24 @@ void turnoff_all() { int rc; + // Immediate feedback: LED off and haptic + led_off(); + // Set is_off immediately so set_led_state() keeps LEDs off + is_off = true; + +#ifdef CONFIG_OMI_ENABLE_HAPTIC + play_haptic_milli(100); + k_msleep(300); + haptic_off(); +#endif + + // Delays for stability + k_msleep(1000); + + // // Enter the low power mode + transport_off(); + k_msleep(300); + // Always turn off microphone mic_off(); k_msleep(100); diff --git a/omi/firmware/omi/src/lib/core/transport.c b/omi/firmware/omi/src/lib/core/transport.c index 4cb3b11fd47..3ee4d17f67e 100644 --- a/omi/firmware/omi/src/lib/core/transport.c +++ b/omi/firmware/omi/src/lib/core/transport.c @@ -389,15 +389,17 @@ static void exchange_func(struct bt_conn *conn, uint8_t att_err, struct bt_gatt_ // Battery Service Handlers // -#define BATTERY_REFRESH_INTERVAL 60000 // 60 seconds - #ifdef CONFIG_OMI_ENABLE_BATTERY +#define BATTERY_REFRESH_INTERVAL 10000 // 10 seconds +#define CONFIG_OMI_BATTERY_CRITICAL_MV 3500 // mV + void broadcast_battery_level(struct k_work *work_item); K_WORK_DELAYABLE_DEFINE(battery_work, broadcast_battery_level); void broadcast_battery_level(struct k_work *work_item) { + static uint8_t notify_counter = 6; uint16_t battery_millivolt; uint8_t battery_percentage; if (battery_get_millivolt(&battery_millivolt) == 0 && @@ -405,10 +407,19 @@ void broadcast_battery_level(struct k_work *work_item) LOG_PRINTK("Battery at %d mV (capacity %d%%)\n", battery_millivolt, battery_percentage); - // Use the Zephyr BAS function to set (and notify) the battery level - int err = bt_bas_set_battery_level(battery_percentage); - if (err) { - LOG_ERR("Error updating battery level: %d", err); + if (battery_millivolt < CONFIG_OMI_BATTERY_CRITICAL_MV) { + LOG_WRN("Battery critical level reached (%d mV). Initiating shutdown.", battery_millivolt); + turnoff_all(); + } else { + notify_counter++; + if (notify_counter >= 6) { + // Use the Zephyr BAS function to set (and notify) the battery level + int err = bt_bas_set_battery_level(battery_percentage); + if (err) { + LOG_ERR("Error updating battery level: %d", err); + } + notify_counter = 0; + } } } else { LOG_ERR("Failed to read battery level"); @@ -460,10 +471,6 @@ static void _transport_connected(struct bt_conn *conn, uint8_t err) update_data_length(current_connection); update_mtu(current_connection); -#ifdef CONFIG_OMI_ENABLE_BATTERY - k_work_schedule(&battery_work, K_MSEC(3000)); -#endif - is_connected = true; } @@ -1036,6 +1043,8 @@ int transport_start() } else { LOG_INF("Battery initialized"); } + + k_work_schedule(&battery_work, K_MSEC(3000)); #endif // Start pusher