Battery Monitor (#1979)

* Modified AggieAir airframes

* Bump on pprzlink

* remove errors from lidar compilation

* Added battery monitor module

* Added support for battery monitor module

* update aggieair conf

conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml

@@ -62,6 +62,12 @@
     <module name="lidar_lite">
       <configure name="LIDAR_LITE_I2C_DEV" value="i2c2"/>
     </module>
+    <module name="battery_monitor.xml">
+      <define name="BATMON_CURRENT_OFFSET" value="-120"/>
+      <define name="BATMON_CURRENT_SENSITIVITY" value="25.6"/>
+      <define name="BATMON_TEMP_OFFSET" value="250"/>
+      <define name="BATMON_TEMP_SENSITIVITY" value="10"/>
+    </module>
   </modules>
 
     <servos driver="Pwm">

conf/airframes/AGGIEAIR/aggieair_conf.xml

@@ -7,7 +7,7 @@
    telemetry="telemetry/AGGIEAIR/aggieair_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic_geofence.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/nps.xml"
-   settings_modules="modules/lidar_lite.xml modules/gps.xml"
+   settings_modules="modules/battery_monitor.xml modules/lidar_lite.xml modules/gps.xml"
    gui_color="#ffff954c0000"
   />
   <aircraft
@@ -29,7 +29,7 @@
    telemetry="telemetry/AGGIEAIR/aggieair_fixedwing.xml"
    flight_plan="flight_plans/AGGIEAIR/BasicTuning_Launcher.xml"
    settings="settings/fixedwing_basic.xml settings/nps.xml settings/control/ctl_basic.xml"
-   settings_modules="modules/lidar_lite.xml modules/nav_skid_landing.xml modules/nav_survey_poly_osam.xml modules/gps.xml"
+   settings_modules="modules/battery_monitor.xml modules/lidar_sf11.xml modules/nav_skid_landing.xml modules/nav_survey_poly_osam.xml modules/gps.xml"
    gui_color="#00009e93ffff"
   />
 </conf>

conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml

@@ -15,9 +15,10 @@ RP3 Lisa MX
 
     <!-- NOTE: if you want to use extra_dl module for HITL
     you have to set TELEMETRY_FREQUENCY to CONTROL_FREQUENCY -->
-    <configure name="PERIODIC_FREQUENCY" value="160"/>
-    <define name="CONTROL_FREQUENCY" value="160"/>
-    <configure name="TELEMETRY_FREQUENCY" value="160"/>
+    <configure name="PERIODIC_FREQUENCY" value="100"/>
+    <define name="CONTROL_FREQUENCY" value="100"/>
+    <configure name="TELEMETRY_FREQUENCY" value="100"/>
+    <define name="SERVO_HZ" value="400"/>
 
     <target name="nps" board="pc">
       <module name="fdm" type="jsbsim"/>
@@ -60,18 +61,25 @@ RP3 Lisa MX
       <configure name="EXTRA_DL_PORT" value="UART1"/>
       <configure name="EXTRA_DL_BAUD" value="B921600"/>
     </module>
-    <module name="lidar_lite"/>
+    <module name="lidar_sf11"/>
+    <module name="battery_monitor.xml">
+      <define name="BATMON_REV4" value="1"/>
+      <define name="BATMON_CURRENT_OFFSET" value="-120"/>
+      <define name="BATMON_CURRENT_SENSITIVITY" value="25.6"/>
+      <define name="BATMON_TEMP_OFFSET" value="250"/>
+      <define name="BATMON_TEMP_SENSITIVITY" value="10"/>
+    </module>
   </modules>
 
   <!-- commands section -->
   <!-- Servo Configuration -->
   <servos>
-    <servo name="THROTTLE"		  no="0" min="1100" neutral="1100" max="1900"/>
+    <servo name="THROTTLE"	no="0" min="1100" neutral="1100" max="1900"/>
     <servo name="AILERON_RIGHT" no="1" min="900" neutral="1500" max="2100"/>
     <servo name="AILERON_LEFT"	no="2" min="2100" neutral="1500" max="900"/>
     <servo name="ELEVATOR"      no="3" min="2100" neutral="1500" max="900"/>
-    <servo name="RUDDER"        no="4" min="900" neutral="1500" max="2100"/>
-    <servo name="FLAP"		      no="5" min="900" neutral="900" max="2100"/>
+    <servo name="RUDDER"        no="4" min="2100" neutral="1500" max="900"/>
+    <servo name="FLAP"		no="5" min="900" neutral="1500" max="2100"/>
   </servos>
 
   <!-- Servo Command Structure -->
@@ -214,7 +222,11 @@ RP3 Lisa MX
     <define name="PITCH_MAX_SETPOINT" value="0.35" unit="rad"/>
     <define name="PITCH_MIN_SETPOINT" value="-0.2" unit="rad"/>
     <define name="PITCH_PGAIN" value="12900."/>
+    <!-- ORIGINAL GAIN
     <define name="PITCH_DGAIN" value="1.5"/>
+    -->
+    <!-- SIMULATOR GAIN -->
+    <define name="PITCH_DGAIN" value="151.5"/>
     <define name="ELEVATOR_OF_ROLL" value="1250"/>
     <define name="ROLL_ATTITUDE_GAIN" value="9000"/>
     <define name="ROLL_RATE_GAIN" value="1600"/>
@@ -258,7 +270,7 @@ RP3 Lisa MX
 
   <section name="SIMULATOR" prefix="NPS_">
     <define name="JSBSIM_LAUNCHSPEED" value="25"/>
-    <define name="JSBSIM_MODEL" value="easystar" type="string"/>
+    <define name="JSBSIM_MODEL" value="minion" type="string"/>
     <define name="SENSORS_PARAMS" value="nps_sensors_params_invariant.h" type="string"/>
     <define name="JS_AXIS_THROTTLE" value="0"/>
     <define name="JS_AXIS_THROTTLE_REVERSED" value="1"/>

conf/modules/battery_monitor.xml

@@ -0,0 +1,68 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="battery_monitor" dir="adcs">
+  <doc>
+    <description>
+      AggieAir Battery and power monitor using a custom breakout board, for both fixedwings and rotocrafts.
+      Both versions use TMP35 temperature sensor for monitoring board and cell temperatures, AD7998 (12bit) ADC
+      for version 4 and AD7997 (10bit) ADC for version 5, and ACS758 bidirectional current sensor.
+
+      There are two versions:
+      - Rev 4: ADC VIN7 unused
+      - Rev 5: on ADC VIN7 is now ARMED_IN (channel that sensing that the safety plug is plugged in.
+      Define if you are using BATTERY_MONITOR_REV4 in your airfame config file (otherwise rev5 is assumed).
+      Also the versions differ in the gain coefficients.
+    </description>
+    <configure name="BATTERY_MONITOR_I2C_DEV" value="i2c2" description="I2C device to use for the monitor"/>
+    <configure name="BATTERY_MONITOR_BUS_ADC_I2C_ADDR" value="0x42" description="slave address of bus adc"/>
+    <configure name="BATTERY_MONITOR_BALANCE_BAT1_ADC_I2C_ADDR" value="0x40" description="slave address of balance 1 adc"/>
+    <configure name="BATTERY_MONITOR_BALANCE_BAT2_ADC_I2C_ADDR" value="0x44" description="slave address of balance 2 adc"/>
+    <configure name="BATTERY_MONITOR_REV4" value="1" description="set to 1 if using Rev.4"/>
+    <configure name="BATTERY_MONITOR_CURRENT_OFFSET" value="-120" description="offset from the curremt sensor"/>
+    <configure name="BATTERY_MONITOR_CURRENT_SENSITIVITY" value="25.6"/>
+    <configure name="BATTERY_MONITOR_TEMP_OFFSET" value="250" description="offset from the temperature sensor"/>
+    <configure name="BATTERY_MONITOR_TEMP_SENSITIVITY" value="10"/>
+  </doc>
+
+  <settings>
+	  <dl_settings>
+  		<dl_settings NAME="Batery monitor">
+	  		<dl_setting MIN="-2000" MAX="2000" STEP="1" VAR="batmon_current_offset" shortname="cur_off" module="modules/adcs/battery_monitor"/>
+	  		<dl_setting MIN="0" MAX="50" STEP="0.1" VAR="batmon_current_sensitivity" shortname="cur_sens" module="modules/adcs/battery_monitor"/>
+	  		<dl_setting MIN="-2000" MAX="2000" STEP="1" VAR="batmon_temp_offset" shortname="temp_off" module="modules/adcs/battery_monitor"/>
+	  		<dl_setting MIN="0" MAX="50" STEP="0.1" VAR="batmon_temp_sensitivity" shortname="temp_sens" module="modules/adcs/battery_monitor"/>
+	  	</dl_settings>
+	  </dl_settings>
+  </settings>
+
+  <header>
+    <file name="battery_monitor.h"/>
+  </header>
+
+  <init fun="battery_monitor_init()"/>
+  <periodic fun="battery_monitor_read_bus()" freq="10"/>
+  <periodic fun="battery_monitor_read_balance_ports_1()" freq="10"/>
+  <periodic fun="battery_monitor_read_balance_ports_2()" freq="10"/>
+  <event fun="battery_monitor_event()"/>
+  <makefile target="ap|fbw">
+    <file name="battery_monitor.c"/>
+    <define name="USE_I2C2"/>
+    <define name="USE_BATTERY_MONITOR"/>
+
+    <configure name="BATTERY_MONITOR_I2C_DEV" default="i2c2" case="lower|upper"/>
+    <define name="BATTERY_MONITOR_I2C_DEV" value="$(BATTERY_MONITOR_I2C_DEV_LOWER)"/>
+    <define name="USE_$(BATTERY_MONITOR_I2C_DEV_UPPER)"/>
+
+    <configure name="BATTERY_MONITOR_REV4" default="0"/>
+    <define name="BATTERY_MONITOR_REV4" value="$(BATTERY_MONITOR_REV4)"/>
+
+    <configure name="BATTERY_MONITOR_BUS_ADC_I2C_ADDR" default="0x42"/>
+    <define name="BATTERY_MONITOR_BUS_ADC_I2C_ADDR" value="$(BATTERY_MONITOR_BUS_ADC_I2C_ADDR)"/>
+
+    <configure name="BATTERY_MONITOR_BALANCE_BAT1_ADC_I2C_ADDR" default="0x40"/>
+    <define name="BATTERY_MONITOR_BALANCE_BAT1_ADC_I2C_ADDR" value="$(BATTERY_MONITOR_BALANCE_BAT1_ADC_I2C_ADDR)"/>
+
+    <configure name="BATTERY_MONITOR_BALANCE_BAT2_ADC_I2C_ADDR" default="0x44"/>
+    <define name="BATTERY_MONITOR_BALANCE_BAT2_ADC_I2C_ADDR" value="$(BATTERY_MONITOR_BALANCE_BAT2_ADC_I2C_ADDR)"/>
+  </makefile>
+</module>

sw/airborne/modules/adcs/battery_monitor.c

@@ -0,0 +1,361 @@
+/*
+ * Copyright (C) 2008-2015 The Paparazzi Team
+ * 2017, Utah State University, http://aggieair.usu.edu/
+ * Michal Podhradsky, michal.podhradsky@aggiemail.usu.edu
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/** @file modules/adcs/battery_monitor.c
+ * driver for ADC AD7997 on a custom made power board version 4.0 and 5.0
+ *
+ */
+#include "modules/adcs/battery_monitor.h"
+
+#if BATTERY_MONITOR_REV4
+PRINT_CONFIG_MSG("Battery monitor: using older revision 4");
+#endif /* BATTERY_MONITOR_REV4 */
+
+struct BatMonBus batmonbus;
+struct BatMonBal batmonbal1;
+struct BatMonBal batmonbal2;
+
+// can be tuned via datalink
+int16_t batmon_current_offset;
+float batmon_current_sensitivity;
+int16_t batmon_temp_offset;
+float batmon_temp_sensitivity;
+
+#if PERIODIC_TELEMETRY
+#include "subsystems/datalink/telemetry.h"
+static void send_batmon(struct transport_tx *trans, struct link_device *dev)
+{
+  static uint16_t power_status;
+#if BATTERY_MONITOR_REV4
+  /* revision 4 */
+  static uint8_t version = 1;
+  power_status = 0; // no status
+#else
+  /* revision 5 */
+  static uint8_t version = 2;
+  power_status = batmonbus.bus_tempsensors_mvolts[4]; // VIN7 has position 5 in temp map, hence index 4
+#endif
+
+  uint8_t batmonbus_bus_trans_status = batmonbus.bus_trans.status;
+  uint8_t batmonbal1_bus_trans_status = batmonbal1.bus_trans.status;
+  uint8_t batmonbal2_bus_trans_status = batmonbal2.bus_trans.status;
+  pprz_msg_send_BATTERY_MONITOR(trans, dev, AC_ID,
+        &version,
+        &batmonbus.bus_status,
+	      &batmonbus_bus_trans_status,
+	      &batmonbus.bus_current_mvolts,
+	      &batmonbus.bus_current,
+	      &batmonbus.bus_voltage_mvolts,
+	      TEMP_SENSORS_NB,
+	      batmonbus.bus_tempsensors_mvolts,
+	      TEMP_SENSORS_NB,
+	      batmonbus.bus_temp,
+	      &batmonbal1_bus_trans_status,
+	      BATTERY_CELLS_NB,
+	      batmonbal1.bat_cell_mvolts,
+	      &batmonbal2_bus_trans_status,
+	      BATTERY_CELLS_NB,
+	      batmonbal2.bat_cell_mvolts,
+	      &power_status);
+}
+#endif
+
+/**
+ *
+ * Translates the channel number to the value
+ * for address pointer register
+ * Channels are numbered from 1 to 8
+ * @param chan - desired channel
+ * @return addres pointer Byte (see AD7997/8 datasheet)
+ */
+uint8_t battery_monitor_get_address(uint8_t chan) {
+  if ((chan>8) || (chan<1)){ // sanity check
+    return 0x80;
+  }
+  static uint8_t adr[] = {0x80, 0x90, 0xA0, 0xB0, 0xC0, 0xD0, 0xE0, 0xF0};
+  return adr[chan-1];
+}
+
+/**
+ * Initializes bus ADC
+ */
+void battery_monitor_init_bus(void){
+  // can be tuned via datalink
+  batmon_current_offset = BATTERY_MONITOR_CURRENT_OFFSET;
+  batmon_current_sensitivity = BATTERY_MONITOR_CURRENT_SENSITIVITY;
+  batmon_temp_offset = BATTERY_MONITOR_TEMP_OFFSET;
+  batmon_temp_sensitivity = BATTERY_MONITOR_TEMP_SENSITIVITY;
+
+  // init Bus ADC
+  // transactions
+  batmonbus.bus_trans.status = I2CTransDone;
+  batmonbus.addr = BATTERY_MONITOR_BUS_ADC_I2C_ADDR;
+  batmonbus.bus_status = BATTERY_MONITOR_BUS_CURRENT; // device status
+  // Current readings
+  batmonbus.bus_current_mvolts = 0; // mV
+  batmonbus.bus_current = 0; // A
+  // Bus voltage readings
+  batmonbus.bus_voltage_mvolts = 0; //mV
+  // Temperature readings
+  batmonbus.bus_brd_tmp = 0; // C
+  memset(batmonbus.bus_tempsensors_mvolts, 0, sizeof(batmonbus.bus_tempsensors_mvolts));
+  memset(batmonbus.bus_temp, 0, sizeof(batmonbus.bus_temp));
+  batmonbus.t_idx = 0; // temp sensor index
+  batmonbus.data = 0;
+}
+
+/**
+ * Initalizes balance ADC 1
+ */
+void battery_monitor_init_balance(struct BatMonBal* batmonbal){
+  batmonbal->bus_trans.status = I2CTransDone;
+  batmonbal->cell_index = 0;
+  memset(batmonbal->bat_cell_mvolts, 0, sizeof(batmonbal->bat_cell_mvolts));
+  batmonbal->data = 0;
+}
+
+/**
+ * Init variables
+ */
+void battery_monitor_init(void) {
+  // bus ADC
+  battery_monitor_init_bus();
+
+  // balance 1 ADC
+  battery_monitor_init_balance(&batmonbal1);
+  batmonbal1.addr = BATTERY_MONITOR_BALANCE_BAT1_ADC_I2C_ADDR;
+
+  // balance 2 ADC
+  battery_monitor_init_balance(&batmonbal2);
+  batmonbal2.addr = BATTERY_MONITOR_BALANCE_BAT2_ADC_I2C_ADDR;
+
+#if PERIODIC_TELEMETRY
+  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_BATTERY_MONITOR, send_batmon);
+#endif
+}
+
+/**
+ * Event function
+ */
+void battery_monitor_event(void){
+  // Empty for now
+}
+
+/**
+ * Read bus (current, voltage and temperature sensors)
+ */
+void battery_monitor_read_bus(void){
+  batmonbus.data = 0; // erase at each iteration
+
+  switch (batmonbus.bus_status) {
+    case BATTERY_MONITOR_BUS_CURRENT:
+      // set ADC to current channel
+      batmonbus.bus_trans.buf[0] = battery_monitor_get_address(
+                                   (uint8_t)BATTERY_MONITOR_BUS_CURRENT_CHANNEL);
+      //set to zero so we can detect an error
+      batmonbus.bus_current = 0;
+      // blocking transaction
+      if (i2c_transceive(&BATTERY_MONITOR_I2C_DEV, &batmonbus.bus_trans, batmonbus.addr, 1, 2)){
+        // proceed only if transaction was submitted successfully
+        if (batmonbus.bus_trans.status == I2CTransSuccess) {
+          // read data
+          batmonbus.data = (uint16_t) (batmonbus.bus_trans.buf[0] << 8 | batmonbus.bus_trans.buf[1]);
+          // NOTE: we are not using the ALERT_FLAG at the moment,
+          // get counts
+          batmonbus.bus_current_mvolts = batmonbus.data & 0xFFF;
+          // shift right by 2 bits if 10-bit reads only
+          if (BATTERY_MONITOR_BIT_RES == 10){
+            batmonbus.bus_current_mvolts = (batmonbus.bus_current_mvolts) >> 2;
+          }
+          // convert to mV
+          batmonbus.bus_current_mvolts = (uint16_t)(
+                                         (float)batmonbus.bus_current_mvolts * BATTERY_MONITOR_VREF_MULT);
+          // convert to [A]
+          batmonbus.bus_current = ((float)batmonbus.bus_current_mvolts +
+                                   (float)batmon_current_offset) / batmon_current_sensitivity;
+        }
+        // optional: check for errors and reset i2c transaction
+        // shouldn't be needed
+
+        // update status
+        batmonbus.bus_status = BATTERY_MONITOR_BUS_VOLTAGE;
+      }
+      //update electrical subsystem, current in mAs
+      electrical.current = (int32_t)(batmonbus.bus_current*1000);
+      break;
+
+    case BATTERY_MONITOR_BUS_VOLTAGE:
+      // set ADC to voltage channel
+      batmonbus.bus_trans.buf[0] = battery_monitor_get_address(
+                                   (uint8_t)BATTERY_MONITOR_BUS_VOLTAGE_CHANNEL);
+      //set to zero so we can detect an error
+      batmonbus.bus_voltage_mvolts = 0;
+      // blocking transaction
+      if (i2c_transceive(&BATTERY_MONITOR_I2C_DEV, &batmonbus.bus_trans, batmonbus.addr, 1, 2)){
+        // proceed only if transaction was submitted successfully
+        if (batmonbus.bus_trans.status == I2CTransSuccess) {
+          // read data
+          batmonbus.data = (uint16_t) (batmonbus.bus_trans.buf[0] << 8 | batmonbus.bus_trans.buf[1]);
+          // NOTE: we are not using the ALERT_FLAG at the moment,
+          // get counts
+          batmonbus.bus_voltage_mvolts = batmonbus.data & 0xFFF;
+          // shift right by 2 bits if 10-bit reads only
+          if (BATTERY_MONITOR_BIT_RES == 10){
+            batmonbus.bus_voltage_mvolts = (batmonbus.bus_voltage_mvolts) >> 2;
+          }
+          // convert to mV
+          batmonbus.bus_voltage_mvolts = (uint16_t)(
+                                         (float)batmonbus.bus_voltage_mvolts * BATTERY_MONITOR_VREF_MULT);
+          // convert to actual voltage
+          batmonbus.bus_voltage_mvolts = (uint16_t)(
+                                         (float)batmonbus.bus_voltage_mvolts * BatmonVbusGain);
+        }
+        // optional: check for errors and reset i2c transaction
+        // shouldn't be needed
+
+        // update status
+        batmonbus.bus_status = BATTERY_MONITOR_BUS_TEMPERATURE;
+      }
+
+      //update electrical subsystem, voltage in decivolts
+      if (batmonbus.bus_voltage_mvolts != 0) {
+        electrical.vsupply = (uint16_t)(batmonbus.bus_voltage_mvolts/100);
+      }
+      else {
+        electrical.vsupply = 0;
+      }
+      break;
+
+    case BATTERY_MONITOR_BUS_TEMPERATURE:
+      // set ADC to correct temperature channel
+      batmonbus.bus_trans.buf[0] = battery_monitor_get_address(
+                                   battery_monitor_tempmap[batmonbus.t_idx]);
+      //set to zero so we can detect an error
+      batmonbus.bus_temp[batmonbus.t_idx] = 0;
+      // blocking transaction
+      if (i2c_transceive(&BATTERY_MONITOR_I2C_DEV, &batmonbus.bus_trans, batmonbus.addr, 1, 2)){
+        // proceed only if transaction was submitted successfully
+        if (batmonbus.bus_trans.status == I2CTransSuccess) {
+          // read data
+          batmonbus.data = (uint16_t) (batmonbus.bus_trans.buf[0] << 8 | batmonbus.bus_trans.buf[1]);
+          // NOTE: we are not using the ALERT_FLAG at the moment,
+          // get counts
+          batmonbus.bus_tempsensors_mvolts[batmonbus.t_idx] = batmonbus.data & 0xFFF;
+          // shift right by 2 bits if 10-bit reads only
+          if (BATTERY_MONITOR_BIT_RES == 10){
+            batmonbus.bus_tempsensors_mvolts[batmonbus.t_idx] =
+                (batmonbus.bus_tempsensors_mvolts[batmonbus.t_idx]) >> 2;
+          }
+          // convert to mV
+          batmonbus.bus_tempsensors_mvolts[batmonbus.t_idx] = (uint16_t)(
+              (float)batmonbus.bus_tempsensors_mvolts[batmonbus.t_idx] * BATTERY_MONITOR_VREF_MULT);
+          // convert to temperature[C]
+          batmonbus.bus_temp[batmonbus.t_idx] =
+              ((float)batmonbus.bus_tempsensors_mvolts[batmonbus.t_idx] +
+                  (float)batmon_temp_offset ) / batmon_temp_sensitivity;
+        }
+      }
+      // optional: check for errors and reset i2c transaction
+      // shouldn't be needed
+
+      // increment counter
+      batmonbus.t_idx++;
+      if (batmonbus.t_idx == TEMP_SENSORS_NB) {
+        batmonbus.t_idx = 0;
+        batmonbus.bus_status = BATTERY_MONITOR_BUS_CURRENT; // to loop through
+      }
+      break;
+    default:
+      // a recovery in case of a glitch
+      batmonbus.bus_status = BATTERY_MONITOR_BUS_CURRENT;
+      break;
+  }
+}
+
+
+/**
+ * Read Balance ADC 1
+ */
+void battery_monitor_read_balance_ports_1(void) {
+  battery_monitor_read_balance_ports(&batmonbal1);
+}
+
+/**
+ * Read Balance ADC 2
+ */
+void battery_monitor_read_balance_ports_2(void) {
+  battery_monitor_read_balance_ports(&batmonbal2);
+}
+
+/**
+ * Read balance ADC
+ */
+void battery_monitor_read_balance_ports(struct BatMonBal* batmonbal) {
+  batmonbal->data = 0; // erase at each iteration
+
+  // set the right address into ADC
+  // cell_index is the number of the cell we want to probe
+  // battery_monitor_cellmap1 contains the mapping of channels for given cell number
+  // i.e. Cell_1 (cell_idx = 0) has channel 2
+  // this gets translated into hexadecimal number representing the channel internally
+  batmonbal->bus_trans.buf[0] =
+      battery_monitor_get_address((uint8_t)battery_monitor_cellmap[batmonbal->cell_index]);
+
+  //set to zero so we can detect an error
+  batmonbal->bat_cell_mvolts[batmonbal->cell_index] = 0;
+
+  // blocking transaction
+  if (i2c_transceive(&BATTERY_MONITOR_I2C_DEV, &batmonbal->bus_trans, batmonbal->addr, 1, 2)){
+    // proceed only if transaction was submitted successfully
+    if (batmonbal->bus_trans.status == I2CTransSuccess) {
+      // read data
+      batmonbal->data = (uint16_t) (batmonbal->bus_trans.buf[0] << 8 | batmonbal->bus_trans.buf[1]);
+      // NOTE: we are not using the ALERT_FLAG at the moment,
+      // get counts
+      batmonbal->bat_cell_mvolts[batmonbal->cell_index] = batmonbal->data & 0xFFF;
+      // shift right by 2 bits if 10-bit reads only
+      if (BATTERY_MONITOR_BIT_RES == 10){
+        batmonbal->bat_cell_mvolts[batmonbal->cell_index] =
+            (batmonbal->bat_cell_mvolts[batmonbal->cell_index]) >> 2;
+      }
+      // convert to mV
+      batmonbal->bat_cell_mvolts[batmonbal->cell_index] =
+          (uint16_t)((float)batmonbal->bat_cell_mvolts[batmonbal->cell_index] * BATTERY_MONITOR_VREF_MULT);
+      // convert to actual voltage
+      // we get the multiplier from battery_monitor_cellgains array, which contains multipliers
+      // for cells 1-6(in this order) battery_monitor_cellgains[0] gives us multiplier for cell_1
+      // and so on
+      batmonbal->bat_cell_mvolts[batmonbal->cell_index] =
+          (uint16_t)((float)batmonbal->bat_cell_mvolts[batmonbal->cell_index] *
+              battery_monitor_cellgains[batmonbal->cell_index]);
+    }
+    // optional: check for errors and reset i2c transaction
+    // shouldn't be needed
+  }
+
+  // increment counter
+  batmonbal->cell_index++;
+  if (batmonbal->cell_index == BATTERY_CELLS_NB) {
+    batmonbal->cell_index = 0;
+  }
+}

sw/airborne/modules/adcs/battery_monitor.h

@@ -0,0 +1,261 @@
+/*
+ * Copyright (C) 2008-2015 The Paparazzi Team
+ * 2017, Utah State University, http://aggieair.usu.edu/
+ * Michal Podhradsky, michal.podhradsky@aggiemail.usu.edu
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/** @file modules/adcs/battery_monitor.h
+ * driver for ADC AD7997 on a custom made power board version 4.0 and 5.0
+ *
+ * Power board has:
+ *
+ * BUS_ADC
+ * two possible addresses:
+ * 010 0001 (AD7997-0) = 0x21 (pprz: 0x42)
+ * 010 0011 (AD7997-1) = 0x23 (pprz: 0x46)
+ * channels:
+ * VIN1 - Temp_1
+ * VIN2 - Current sensor
+ * VIN3 - PWR_BRD_Temp
+ * VIN4 - Vin
+ * VIN5 - Temp_2
+ * VIN6 - Batt_Temp1
+ * VIN7 - Temp_3
+ * VIN8 - Batt_temp_2
+ *
+ * BALANCE_ADC_1 (battery 1)
+ * address:
+ * 010 0000 (AD7997-1 or AD7997-0) = 0x20 (pprz: 0x40)
+ * channels:
+ * VIN1 - NC
+ * VIN2 - Cell_1
+ * VIN3 - NC
+ * VIN4 - Cell_2
+ * VIN5 - Cell_5
+ * VIN6 - Cell_3
+ * VIN7 - Cell_6
+ * VIN8 - Cell_4
+ *
+ * BALANCE_ADC_2 (battery 2)
+ * address:
+ * 010 0010 (AD7997-0) = 0x22 (pprz: 0x44)
+ * 010 0100 (AD7997-1) = 0x24 (pprz: 0x48)
+ * channels:
+ * VIN1 - NC
+ * VIN2 - Cell_1
+ * VIN3 - NC
+ * VIN4 - Cell_2
+ * VIN5 - NC
+ * VIN6 - Cell_3
+ * VIN7 - NC
+ * VIN8 - Cell_4
+ */
+#ifndef BATTERY_MONITOR_H
+#define BATTERY_MONITOR_H
+
+#include "std.h"
+#include "mcu_periph/i2c.h"
+#include "mcu_periph/sys_time.h"
+#include "subsystems/electrical.h"
+
+// revision information
+#if BATTERY_MONITOR_REV4
+/* revision 4 */
+#define BATTERY_MONITOR_BIT_RES 10 // define bit resolution: 12 for AD7998 (rev4)
+#else
+/* revision 5 */
+#define BATTERY_MONITOR_BIT_RES 12 // define bit resolution: 10 for AD7997 (rev5)
+#endif /* BATTERY_MONITOR_REV4 */
+
+// default reference (5 Volts)
+#define BATTERY_MONITOR_VREF 5000 //mV
+
+// reading multiplier
+#define BATTERY_MONITOR_VREF_MULT  BATTERY_MONITOR_VREF/(1<<BATTERY_MONITOR_BIT_RES)
+
+// we are using 6 cells by default
+#define BATTERY_CELLS_NB 6
+
+// we are using 6 temp sensors right now
+#define TEMP_SENSORS_NB 6
+
+/*
+ *  cell ADC gains
+ * representing inverse values of the voltage dividers
+ */
+#if BATTERY_MONITOR_REV4
+/* revision 4 */
+#define GAIN_CELL_1 1.001
+#define GAIN_CELL_2 2.011
+#define GAIN_CELL_3 3.027
+#define GAIN_CELL_4 4.063
+#define GAIN_CELL_5 4.217
+#define GAIN_CELL_6 4.973
+#else
+/* revision 5 */
+#define GAIN_CELL_1 1.0
+#define GAIN_CELL_2 2.0
+#define GAIN_CELL_3 3.0
+#define GAIN_CELL_4 4.03
+#define GAIN_CELL_5 5.0
+#define GAIN_CELL_6 6.0
+#endif /* BATTERY_MONITOR_REV4 */
+static const float battery_monitor_cellgains[] = {GAIN_CELL_1, GAIN_CELL_2,
+    GAIN_CELL_3, GAIN_CELL_4, GAIN_CELL_5, GAIN_CELL_6};
+
+/**
+ * Cell map - which cell is which channel
+ * Channels are 1-indexed, ie.e. Channel 1 - Channel 8
+ * Cells are 0-indexed (because of their position in the array)
+ * So cell at battery_monitor_cellmap[0] is the first cell etc.
+ */
+static const uint8_t battery_monitor_cellmap[] = {2,4,6,8,5,7}; // 6s cellmap (battery1)
+// for 4 cell battery the last two readings should be zero (channel tied down to GND)
+// or HIGH (channel tied to VCC)
+
+// define channels
+#define BATTERY_MONITOR_BUS_CURRENT_CHANNEL 2
+#define BATTERY_MONITOR_BUS_VOLTAGE_CHANNEL 4
+
+/*
+ * Channels for the temp sensors
+ */
+static const uint8_t battery_monitor_tempmap[] = {1,3,5,6,7,8};
+
+/*
+ * Define bus voltage gain based on
+ * values of voltage divider at
+ * power board
+ */
+#if BATTERY_MONITOR_REV4
+/* revision 4 */
+static const float BatmonVbusGain = 4.953980661;
+#else
+/* revision 5 */
+static const float BatmonVbusGain = 6.0;
+#endif /* BATTERY_MONITOR_REV4 */
+
+/*
+ * Define current sensor calibration
+ * include default values
+ * See datasheet for your current sensor
+ * to get the typical values
+ */
+#ifndef BATTERY_MONITOR_CURRENT_OFFSET //mV
+#ifdef BATTERY_MONITOR_REV4
+/* revision 4 */
+#define BATTERY_MONITOR_CURRENT_OFFSET -120
+#else
+/* revision 5 */
+#define BATTERY_MONITOR_CURRENT_OFFSET -657
+#endif /* BATTERY_MONITOR_REV4 */
+#endif /* BATTERY_MONITOR_CURRENT_OFFSET */
+
+#ifndef BATTERY_MONITOR_CURRENT_SENSITIVITY //mV/A
+#define BATTERY_MONITOR_CURRENT_SENSITIVITY 25.6
+#endif
+
+/*
+ * Calibration for the temperature sensors
+ * (for now just one for all of them)
+ * values for TMP35
+ */
+#ifndef BATTERY_MONITOR_TEMP_OFFSET //mV
+#define BATTERY_MONITOR_TEMP_OFFSET 250
+#endif
+#ifndef BATTERY_MONITOR_TEMP_SENSITIVITY //mV/C
+#define BATTERY_MONITOR_TEMP_SENSITIVITY 10
+#endif
+
+// can be tuned via datalink
+extern int16_t batmon_current_offset;
+extern float batmon_current_sensitivity;
+extern int16_t batmon_temp_offset;
+extern float batmon_temp_sensitivity;
+
+
+/**
+ * Status for Bus ADC
+ */
+enum BatmonBusStatus {
+  BATTERY_MONITOR_BUS_CURRENT,
+  BATTERY_MONITOR_BUS_VOLTAGE,
+  BATTERY_MONITOR_BUS_TEMPERATURE
+};
+
+/**
+ * Battery monitor
+ * Bus ADC struct
+ */
+struct BatMonBus {
+  struct i2c_transaction bus_trans; // transaction
+  enum BatmonBusStatus bus_status; // device status
+  uint16_t data; // raw 16bit read from adc
+  uint8_t addr; // i2c device address
+
+  // Current readings
+  uint16_t bus_current_mvolts; // mV
+  float bus_current; // A
+
+  // Bus voltage readings
+  uint16_t bus_voltage_mvolts; //mV
+
+  // Temperature readings
+  float bus_brd_tmp; // C
+  uint16_t bus_tempsensors_mvolts[TEMP_SENSORS_NB]; // mV
+  float bus_temp[TEMP_SENSORS_NB]; // C
+  uint8_t t_idx; // temp sensor index
+
+};
+
+/**
+ * Battery monitor
+ * Balance ADC struct
+ */
+struct BatMonBal {
+  struct i2c_transaction bus_trans; // transaction
+  uint16_t data; // raw 16bit read from adc
+  uint8_t addr; // i2c device address
+
+  //Balance ADC data
+  uint16_t bat_cell_mvolts[BATTERY_CELLS_NB]; // mV
+
+  // index for reading cells
+  uint8_t cell_index;
+};
+
+extern struct BatMonBus batmonbus;
+extern struct BatMonBal batmonbal1;
+extern struct BatMonBal batmonbal2;
+
+void battery_monitor_init(void);
+void battery_monitor_init_bus(void);
+void battery_monitor_init_balance(struct BatMonBal *);
+
+void battery_monitor_read_bus(void);
+void battery_monitor_read_balance_ports_1(void);
+void battery_monitor_read_balance_ports_2(void);
+void battery_monitor_read_balance_ports(struct BatMonBal *);
+
+void battery_monitor_event(void);
+
+uint8_t battery_monitor_get_address(uint8_t channel);
+
+#endif /* BATTERY_MONITOR_H */

sw/airborne/modules/lidar/lidar_lite.c

@@ -171,8 +171,9 @@ void lidar_lite_periodic(void)
 void lidar_lite_downlink(void)
 {
   uint8_t trans = lidar_lite.trans.status;
+  uint8_t status = lidar_lite.status;
   DOWNLINK_SEND_LIDAR(DefaultChannel, DefaultDevice,
       &lidar_lite.distance,
-      &lidar_lite.status,
+      &status,
       &trans);
 }

sw/airborne/modules/lidar/lidar_sf11.c

@@ -132,8 +132,9 @@ void lidar_sf11_periodic(void)
 void lidar_sf11_downlink(void)
 {
   uint8_t trans = lidar_sf11.trans.status;
+  uint8_t status = lidar_sf11.status;
   DOWNLINK_SEND_LIDAR(DefaultChannel, DefaultDevice,
       &lidar_sf11.distance,
-      &lidar_sf11.status,
+      &status,
       &trans);
 }

sw/airborne/subsystems/electrical.c

@@ -124,7 +124,7 @@ void electrical_periodic(void)
   static uint32_t bat_critical_counter = 0;
   static bool vsupply_check_started = false;
 
-#if defined(ADC_CHANNEL_VSUPPLY) && !defined(SITL)
+#if defined(ADC_CHANNEL_VSUPPLY) && !defined(SITL) && !USE_BATTERY_MONITOR
   electrical.vsupply = 10 * VoltageOfAdc((electrical_priv.vsupply_adc_buf.sum /
                                           electrical_priv.vsupply_adc_buf.av_nb_sample));
 #endif