Update voltage divider math

[hamishfagg]

This resulted in very accurate readings for me so thought I'd share

[longer1996]

I'm adding code modified by ChatGPT and checked by me with added arming via PWM channel (pin D2) and filtering of voltage measurement. In my case, measurement for 4s with resistors R1=10k and R2=2.2k.

/************************************************************************************`
  Original code: Richard Amiss
  Modifications: improved PWM handling, LEDs, debugging, restored OSD, 
                 added PWM anti-shake function, improved voltage measurement for 3.3V ADC,
                 added low-pass filter for voltage measurement.
************************************************************************************/

#include "MSP.h"
#include "MSP_OSD.h"
#include "OSD_positions_config.h"

#define ANALOG_IN                A0
#define VREF                     3.3f    // ADC reference voltage (for XIAO it is 3.3V)
#define VOLT_DIVIDER             54.2f   // Corrected voltage divider value

#define PWM_INPUT_PIN             2
#define ARM_THRESHOLD             1600
#define DISARM_THRESHOLD          1200
#define PWM_STABILITY_THRESHOLD   500     // 0.5 seconds of PWM signal stability

#define FC_FIRMWARE_NAME          "Betaflight"
#define FC_FIRMWARE_IDENTIFIER    "BTFL"

//#define DEBUG                     // Uncomment if you want debugging over USB

HardwareSerial &mspSerial = Serial1;
MSP msp;

uint32_t unarmedMillis = 3000;
uint8_t vbat = 0;
uint32_t flightModeFlags = 0x00000002;
uint32_t custom_mode = 0;
uint32_t previousFlightMode = 0;
uint8_t batteryCellCount = 3;

volatile uint32_t pwm_value = 0;
volatile uint32_t last_rising_edge = 0;
bool pwm_valid = false;
bool pwm_present = false;
uint32_t pwm_stable_time = 0;

char fcVariant[5] = "BTFL";
char craftname[15] = "Craft";

msp_battery_state_t battery_state = { 0 };
msp_name_t name = { 0 };
msp_fc_version_t fc_version = { 0 };
msp_fc_variant_t fc_variant = { 0 };
msp_status_DJI_t status_DJI = { 0 };
msp_analog_t analog = { 0 };

// --- Voltage low-pass filter ---
float filtered_voltage = 0.0f;
const float voltage_filter_factor = 0.025f; // Value from 0.0 to 1.0, smaller means stronger smoothing

void setup() {
  Serial.begin(115200);

  Serial1.begin(115200);
  while (!Serial1);

  analogReadResolution(12);  // 12-bit ADC: values from 0 to 4095

  msp.begin(mspSerial);

  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(PWM_INPUT_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(PWM_INPUT_PIN), pwm_interrupt, CHANGE);

  delay(1000);

  status_DJI.cycleTime = 0x0080;
  status_DJI.i2cErrorCounter = 0;
  status_DJI.sensor = 0x23;
  status_DJI.configProfileIndex = 0;
  status_DJI.averageSystemLoadPercent = 7;
  status_DJI.accCalibrationAxisFlags = 0;
  status_DJI.DJI_ARMING_DISABLE_FLAGS_COUNT = 20;
  status_DJI.djiPackArmingDisabledFlags = (1 << 24);
  flightModeFlags = 0x00000002; // disarmed

#ifdef DEBUG
  Serial.println("Initialized");
#endif
}

void loop() {
  uint32_t currentMillis = millis();

  // Voltage measurement
  vbat = read_battery_voltage();

  digitalWrite(LED_BUILTIN, (flightModeFlags & 0x01) ? LOW : HIGH);

  if (pwm_valid) {
    if (pwm_value >= ARM_THRESHOLD) {
      pwm_stable_time = (pwm_stable_time == 0) ? currentMillis : pwm_stable_time;
      if (currentMillis - pwm_stable_time >= PWM_STABILITY_THRESHOLD) {
        set_flight_mode_flags(true);
        pwm_present = true;
      }
    } else if (pwm_value <= DISARM_THRESHOLD) {
      pwm_stable_time = 0;
      if (pwm_present) {
        set_flight_mode_flags(false);
        pwm_present = false;
      }
    }
  }

  if (custom_mode != previousFlightMode) {
    previousFlightMode = custom_mode;
    display_flight_mode();
  }

  send_msp_to_airunit();

#ifdef DEBUG
  debugPrint();
#endif

  delay(50);
}

void pwm_interrupt() {
  if (digitalRead(PWM_INPUT_PIN) == HIGH) {
    last_rising_edge = micros();
  } else {
    uint32_t pulse_width = micros() - last_rising_edge;
    if (pulse_width > 800 && pulse_width < 2200) {
      pwm_value = pulse_width;
      pwm_valid = true;
    }
  }
}

void set_flight_mode_flags(bool arm) {
  if (arm && (flightModeFlags & 0x01) == 0) {
    flightModeFlags |= 0x01;
#ifdef DEBUG
    Serial.println("ARMED");
#endif
  } else if (!arm && (flightModeFlags & 0x01)) {
    flightModeFlags &= ~0x01;
#ifdef DEBUG
    Serial.println("DISARMED");
#endif
  }
}

void send_msp_to_airunit() {
  memcpy(fc_variant.flightControlIdentifier, fcVariant, sizeof(fcVariant));
  msp.send(MSP_FC_VARIANT, &fc_variant, sizeof(fc_variant));

  fc_version.versionMajor = 4;
  fc_version.versionMinor = 1;
  fc_version.versionPatchLevel = 1;
  msp.send(MSP_FC_VERSION, &fc_version, sizeof(fc_version));

  memcpy(name.craft_name, craftname, sizeof(craftname));
  msp.send(MSP_NAME, &name, sizeof(name));

  status_DJI.flightModeFlags = flightModeFlags;
  status_DJI.armingFlags = 0x0303;
  msp.send(MSP_STATUS_EX, &status_DJI, sizeof(status_DJI));

  status_DJI.armingFlags = 0x0000;
  msp.send(MSP_STATUS, &status_DJI, sizeof(status_DJI));

  battery_state.amperage = 0;
  battery_state.batteryVoltage = vbat * 10;  // unit: 0.1V
  battery_state.mAhDrawn = 0;
  battery_state.batteryCellCount = batteryCellCount;
  battery_state.batteryCapacity = 0;
  battery_state.batteryState = 0;
  battery_state.legacyBatteryVoltage = vbat;
  msp.send(MSP_BATTERY_STATE, &battery_state, sizeof(battery_state));
}

// === Modified function with voltage filter ===
uint8_t read_battery_voltage() {
  uint16_t analog_value = analogRead(ANALOG_IN);
  float voltage = (analog_value * VREF / 4095.0f) * VOLT_DIVIDER;

  // Low-pass filter
  filtered_voltage = filtered_voltage * (1.0f - voltage_filter_factor) + voltage * voltage_filter_factor;

  return (uint8_t)filtered_voltage;
}

void display_flight_mode() {
  // Placeholder - you can later add OSD display here
}

void debugPrint() {
  Serial.println("**********************************");
  Serial.print("Flight Mode Flags: ");
  Serial.println(flightModeFlags, HEX);
  Serial.print("PWM Pulse Width: ");
  Serial.println(pwm_value);
  Serial.print("Battery Voltage (V): ");
  Serial.println(vbat);
}

[ramiss]

Thanks @longer1996. However, have you tested this with the air unit and got it to successfully arm and disarm every time? I had also developed the PWM channel code, but the MSP messaging caused issues and the air unit was not reacting consistently. It was simply more reliable to leave it on once.

[longer1996]

Thanks @longer1996. However, have you tested this with the air unit and got it to successfully arm and disarm every time? I had also developed the PWM channel code, but the MSP messaging caused issues and the air unit was not reacting consistently. It was simply more reliable to leave it on once.

I just put it together and haven't flown it yet. It worked as many times as I tested it on the table. If it works, I'll probably forget to share it here, so I preferred to write it right away. If the tests show that it doesn't work, I'll definitely come back here. :)

[longer1996]

I tested the device with the code I provided previously.
While arming and disarming worked every time, there were interruptions in recording during the flight (invisible in the goggles, but visible in the unevenly divided recordings).
So I sat down with ChatGPT again and we improved the code, which I'm pasting below.
I'll let you know after further tests.

/************************************************************************************
  Original code: Richard Amiss
  Modifications: improved PWM handling, LED and debug logic, restored OSD,
                 added PWM signal debounce, improved voltage measurement for 3.3V ADC,
                 added low-pass voltage filter, refined disarm logic
************************************************************************************/

#include "MSP.h"
#include "MSP_OSD.h"
#include "OSD_positions_config.h"

#define ANALOG_IN                A0
#define VREF                     3.3f                    // ADC reference voltage (3.3V for XIAO)
#define VOLT_DIVIDER             54.2f                   // Voltage divider factor

#define PWM_INPUT_PIN             2
#define ARM_THRESHOLD             1600                   // PWM threshold for arming
#define DISARM_THRESHOLD          1200                   // PWM threshold for disarming
#define PWM_STABILITY_THRESHOLD   1000                    // Signal stability time (in ms)

#define FC_FIRMWARE_NAME          "Betaflight"
#define FC_FIRMWARE_IDENTIFIER    "BTFL"

//#define DEBUG                   // Uncomment to enable USB debug output

HardwareSerial &mspSerial = Serial1;
MSP msp;

uint32_t unarmedMillis = 3000;
uint8_t vbat = 0;
uint32_t flightModeFlags = 0x00000002;                 // Default: disarmed
uint32_t custom_mode = 0;
uint32_t previousFlightMode = 0;
uint8_t batteryCellCount = 3;

volatile uint32_t pwm_value = 0;
volatile uint32_t last_rising_edge = 0;
bool pwm_valid = false;
bool pwm_present = false;
uint32_t pwm_stable_time = 0;

char fcVariant[5] = "BTFL";
char craftname[15] = "Craft";

msp_battery_state_t battery_state = { 0 };
msp_name_t name = { 0 };
msp_fc_version_t fc_version = { 0 };
msp_fc_variant_t fc_variant = { 0 };
msp_status_DJI_t status_DJI = { 0 };
msp_analog_t analog = { 0 };

// --- Low-pass filter for voltage ---
float filtered_voltage = 0.0f;
const float voltage_filter_factor = 0.025f;            // Lower = more smoothing

void setup() {
  Serial.begin(115200);
  Serial1.begin(115200);
  while (!Serial1);

  analogReadResolution(12);                            // 12-bit ADC: values from 0 to 4095
  msp.begin(mspSerial);

  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(PWM_INPUT_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(PWM_INPUT_PIN), pwm_interrupt, CHANGE);

  delay(1000);

  // Initial status settings
  status_DJI.cycleTime = 0x0080;
  status_DJI.i2cErrorCounter = 0;
  status_DJI.sensor = 0x23;
  status_DJI.configProfileIndex = 0;
  status_DJI.averageSystemLoadPercent = 7;
  status_DJI.accCalibrationAxisFlags = 0;
  status_DJI.DJI_ARMING_DISABLE_FLAGS_COUNT = 20;
  status_DJI.djiPackArmingDisabledFlags = (1 << 24);
  flightModeFlags = 0x00000002;                         // Default: disarmed

#ifdef DEBUG
  Serial.println("Initialized");
#endif
}

void loop() {
  uint32_t currentMillis = millis();
  static uint32_t disarmCandidateSince = 0;             // Tracks when a disarm condition first appears

  vbat = read_battery_voltage();
  digitalWrite(LED_BUILTIN, (flightModeFlags & 0x01) ? LOW : HIGH); // LED on when armed

  // Check for valid PWM and update arming state accordingly
  if (pwm_valid) {
    if (pwm_value >= ARM_THRESHOLD) {
      pwm_stable_time = (pwm_stable_time == 0) ? currentMillis : pwm_stable_time;
      disarmCandidateSince = 0;

      if (currentMillis - pwm_stable_time >= PWM_STABILITY_THRESHOLD) {
        set_flight_mode_flags(true);
        pwm_present = true;
      }

    } else if (pwm_value <= DISARM_THRESHOLD) {
      pwm_stable_time = 0;

      if (pwm_present) {
        if (disarmCandidateSince == 0) {
          disarmCandidateSince = currentMillis;
        }

        if (currentMillis - disarmCandidateSince >= PWM_STABILITY_THRESHOLD) {
          set_flight_mode_flags(false);
          pwm_present = false;
          disarmCandidateSince = 0;
        }
      }

    } else {
      pwm_stable_time = 0;
      disarmCandidateSince = 0;
    }
  }

  // Check for mode changes to update OSD or state
  if (custom_mode != previousFlightMode) {
    previousFlightMode = custom_mode;
    display_flight_mode();
  }

  // Send current state to Air Unit
  send_msp_to_airunit();

#ifdef DEBUG
  debugPrint();
#endif

  delay(50);
}

// Interrupt handler for capturing PWM pulse width
void pwm_interrupt() {
  if (digitalRead(PWM_INPUT_PIN) == HIGH) {
    last_rising_edge = micros();
  } else {
    uint32_t pulse_width = micros() - last_rising_edge;
    if (pulse_width > 800 && pulse_width < 2200) {
      pwm_value = pulse_width;
      pwm_valid = true;
    }
  }
}

// Sets or clears the armed flag based on the input
void set_flight_mode_flags(bool arm) {
  if (arm && (flightModeFlags & 0x01) == 0) {
    flightModeFlags |= 0x01;                            // Set armed flag
#ifdef DEBUG
    Serial.println("ARMED");
#endif
  } else if (!arm && (flightModeFlags & 0x01)) {
    flightModeFlags &= ~0x01;                           // Clear armed flag
#ifdef DEBUG
    Serial.println("DISARMED");
#endif
  }
}

// Sends MSP data to the Air Unit (DJI)
void send_msp_to_airunit() {
  const uint16_t ARMED_FLAGS = 0x0303;                  // Constant arming flags

  memcpy(fc_variant.flightControlIdentifier, fcVariant, sizeof(fcVariant));
  msp.send(MSP_FC_VARIANT, &fc_variant, sizeof(fc_variant));

  fc_version.versionMajor = 4;
  fc_version.versionMinor = 1;
  fc_version.versionPatchLevel = 1;
  msp.send(MSP_FC_VERSION, &fc_version, sizeof(fc_version));

  memcpy(name.craft_name, craftname, sizeof(craftname));
  msp.send(MSP_NAME, &name, sizeof(name));

  status_DJI.flightModeFlags = flightModeFlags;
  status_DJI.armingFlags = ARMED_FLAGS;                 // Keep constant flags
  msp.send(MSP_STATUS_EX, &status_DJI, sizeof(status_DJI));
  msp.send(MSP_STATUS, &status_DJI, sizeof(status_DJI));

  battery_state.amperage = 0;
  battery_state.batteryVoltage = vbat * 10;             // Unit: 0.1V
  battery_state.mAhDrawn = 0;
  battery_state.batteryCellCount = batteryCellCount;
  battery_state.batteryCapacity = 0;
  battery_state.batteryState = 0;
  battery_state.legacyBatteryVoltage = vbat;
  msp.send(MSP_BATTERY_STATE, &battery_state, sizeof(battery_state));
}

// Reads battery voltage with low-pass filtering
uint8_t read_battery_voltage() {
  uint16_t analog_value = analogRead(ANALOG_IN);
  float voltage = (analog_value * VREF / 4095.0f) * VOLT_DIVIDER;
  filtered_voltage = filtered_voltage * (1.0f - voltage_filter_factor) + voltage * voltage_filter_factor;
  return (uint8_t)filtered_voltage;
}

// Stub function to implement OSD display updates
void display_flight_mode() {
  // Placeholder for future OSD display
}

// Debug output via USB serial
void debugPrint() {
  Serial.println("**********************************");
  Serial.print("Flight Mode Flags: ");
  Serial.println(flightModeFlags, HEX);
  Serial.print("PWM Pulse Width: ");
  Serial.println(pwm_value);
  Serial.print("Battery Voltage (V): ");
  Serial.println(vbat);
}

[ramiss]

@longer1996 nice work. Let me know if this tests out ok and I'll pull in your work to the master.

[longer1996]

@longer1996 nice work. Let me know if this tests out ok and I'll pull in your work to the master.

After about 10 flights, I can confirm that the code I shared in the previous post worked flawlessly 10/10 times.
Arming and disarming works, the video files confirm this (as there's only one from each flight), and the voltage measurement also works smoothly and stably.