battery feedback for libopencm3 builds

lib/param_generator/PARAMS.md

@@ -23,10 +23,11 @@
 Name | Type | Description | Default | Unit | Options | Reboot
 --- | --- | --- | ---:| --- | --- | ---
 BAT_TYPE | uint | See the MAV_BATTERY_TYPE enum | 0 |  | scalar | False
+BAT_PERIOD_US | uint | Time period for the battery voltage to be checked at | 10000 | us | scalar | False
 BAT_FUNC | uint | See the MAV_BATTERY_FUNCTION enum | 0 |  | scalar | False
 BAT_N_CELLS | uint | Number of cells in the battery | 0 |  | scalar | False
-BAT_V_EMPTY | float | Low cell voltage | 3.7 |  | scalar | False
-BAT_V_CHARGED | float | High cell voltage | 4.2 |  | scalar | False
+BAT_V_EMPTY | float | Low cell voltage | 3.7 | V | scalar | False
+BAT_V_CHARGED | float | High cell voltage | 4.2 | V | scalar | False
 BAT_FILTER | float | A smoothing filter for the voltage reading (0->1) | 0.8 |  | scalar | False
 BAT_V_DIV | float | Resistor voltage divider for battery sensing (-1 to use board default) | -1.0 |  | scalar | False
 BAT_LOW_THR | float | Charge state cutoff for low battery remaining (as a percentage) | 0.2 |  | scalar | False

lib/param_generator/definitions/Battery.yaml

@@ -7,6 +7,16 @@ PARAM_BATTERY_TYPE:
     option: "scalar"
     default: 0
 ---
+PARAM_BATTERY_PERIOD:
+  type: "uint"
+  name: "BAT_PERIOD_US"
+  description: "Time period for the battery voltage to be checked at"
+  reboot: false
+  value:
+    option: "scalar"
+    default: 10000
+    unit: "us"
+---
 PARAM_BATTERY_FUNCTION:
   type: "uint"
   name: "BAT_FUNC"
@@ -33,6 +43,7 @@ PARAM_BATTERY_CELL_MIN:
   value:
     option: "scalar"
     default: 3.7
+    unit: "V"
 ---
 PARAM_BATTERY_CELL_MAX:
   type: "float"
@@ -42,6 +53,7 @@ PARAM_BATTERY_CELL_MAX:
   value:
     option: "scalar"
     default: 4.2
+    unit: "V"
 ---
 PARAM_BATTERY_READING_FILTER:
   type: "float"

src/drivers/breezy_naze32_common/drv_battery_voltage.c

@@ -1,7 +1,7 @@
 #include <stdbool.h>
 
-#include "adc.h"
 #include "breezystm32.h"
+#include "adc.h"
 #include "drivers/drv_sensors.h"
 
 bool drv_sensors_battery_monitor_init( void ) {

src/drivers/opencm3_naze32_common/drv_battery_voltage.c

@@ -2,16 +2,92 @@
 
 #include "drivers/drv_sensors.h"
 
-/*
-#include "adc.h"
-#include "breezystm32.h"
-*/
+#include <libopencm3/stm32/rcc.h>
+#include <libopencm3/stm32/gpio.h>
+#include <libopencm3/stm32/adc.h>
+#include <libopencm3/stm32/timer.h>
+#include <libopencm3/cm3/nvic.h>
+
+#include "drivers/drv_status_io.h"
+#include "drivers/drv_system.h"
+
+
+static bool adc_is_calibrated_ = false;
+static uint32_t adc_start_time_ = 0;
+static uint32_t adc_cal_time_ = 0;
+
 
 bool drv_sensors_battery_monitor_init( void ) {
+	adc_is_calibrated_ = false;
+	adc_start_time_  = system_micros();
+	adc_cal_time_ = 0;
+
+	rcc_periph_clock_enable(RCC_ADC1);
+
+	// Make sure the ADC doesn't run during config.
+	adc_power_off(ADC1);
+
+	// We configure everything for one single conversion.
+	adc_disable_scan_mode(ADC1);
+	adc_set_single_conversion_mode(ADC1);
+	adc_disable_external_trigger_regular(ADC1);
+	adc_set_right_aligned(ADC1);
+	// We want to read the temperature sensor, so we have to enable it.
+	adc_enable_temperature_sensor();
+	adc_set_sample_time_on_all_channels(ADC1, ADC_SMPR_SMP_28DOT5CYC);
+
+	adc_power_on(ADC1);
+
+	/*
+	//Wait for ADC starting up.
+	for (int i = 0; i < 800000; i++)    //wait a while
+		__asm__("nop");
+
+	adc_reset_calibration(ADC1);
+	adc_calibrate_async(ADC1); // Will check this later
+	*/
+
+	// Select the channel we want to convert. 16=temperature_sensor.
+	// Only use one channel for now (ADC_Channel_4 = Voltage)
+	uint8_t channel_array[16];
+	channel_array[0] = 4;
+	adc_set_regular_sequence(ADC1, 1, channel_array);
+
 	return true;
 }
 
 uint16_t drv_sensors_battery_monitor_read( void ) {
-	//return adcGetChannel( ADC_EXTERNAL_PAD );
-	return 0;
+	uint16_t voltage = 0;
+
+	// Start cal if device is warmed up
+	if( ( !adc_is_calibrated_ ) &&
+		( system_micros() - adc_start_time_ > 1000000 ) ) {
+		//If we haven't started calibrating
+		//then start calibration
+		if( !adc_cal_time_ ) {
+			adc_cal_time_ = system_micros();
+			adc_reset_calibration(ADC1);
+			adc_calibrate_async(ADC1); // Will check this later
+		} else {
+			// calibration is done
+			if( !adc_is_calibrating(ADC1) ) {
+				adc_is_calibrated_ = true;
+
+				mavlink_queue_broadcast_info( "[SENSOR] ADC cal done, enabling battery voltage" );
+			}
+		}
+	}
+
+	if( adc_is_calibrated_ ) {
+		// Start ADC conversion without trigger
+		adc_start_conversion_direct(ADC1);
+
+		// Wait for end of conversion.
+		while (!(adc_eoc(ADC1)));
+
+		// Read voltage
+		voltage = adc_read_regular(ADC1);
+	}
+
+	return voltage;
 }

src/profiler.c

@@ -116,7 +116,7 @@ static void profiler_send_debug( profiler_ids_t id ) {
 			robin_itoa(numtext, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN, max, 10);
 			strncat(text,numtext,MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN-1);
 			strncat(text,"]",MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN-1);
-			mavlink_queue_broadcast_debug( text );
+			mavlink_queue_broadcast_info( text );
 		}
 	}
 }

src/sensors.c

@@ -472,7 +472,10 @@ lpq_queue_broadcast_msg(&baro_msg_out);
 	_sensors.safety_button.state_db = safety_button_reading;
 
 	//==-- Voltage Monitor
-	if ( get_param_uint( PARAM_BATTERY_CELL_NUM ) > 0 ) {
+	// Continue as long as the number of cells has been set,
+	// and we are ready for a new sample
+	if( ( get_param_uint( PARAM_BATTERY_CELL_NUM ) > 0 )  &&
+		( (time_us - _sensors.voltage_monitor.status.time_read) > get_param_uint(PARAM_BATTERY_PERIOD) ) ) {
 		//_sensors.voltage_monitor.state_raw =
 		// digitalIn(_sensors.voltage_monitor.gpio_p, _sensors.voltage_monitor.pin);
 		_sensors.voltage_monitor.state_raw = drv_sensors_battery_monitor_read();