aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorLorenz Meier <lm@inf.ethz.ch>2013-01-06 11:25:17 +0100
committerLorenz Meier <lm@inf.ethz.ch>2013-01-06 11:25:17 +0100
commitd3fd3d8219179251d10655944992da75abb8932b (patch)
tree25782ae747ddaacf74d610c9b57627ae46e07f7b
parent0ef1d6d7529e9c84969ed6f512f733772bba34a0 (diff)
parent8eb8909a3c24c6028e4945e4a057d6d2f27f3d04 (diff)
downloadpx4-firmware-d3fd3d8219179251d10655944992da75abb8932b.tar.gz
px4-firmware-d3fd3d8219179251d10655944992da75abb8932b.tar.bz2
px4-firmware-d3fd3d8219179251d10655944992da75abb8932b.zip
Merged, compiling
-rw-r--r--apps/commander/commander.c31
-rw-r--r--apps/drivers/boards/px4fmu/px4fmu_adc.c139
-rw-r--r--apps/drivers/boards/px4fmu/px4fmu_init.c23
-rw-r--r--apps/drivers/boards/px4io/px4io_init.c3
-rw-r--r--apps/drivers/boards/px4io/px4io_internal.h5
-rw-r--r--apps/drivers/drv_adc.h52
-rw-r--r--apps/drivers/px4io/px4io.cpp25
-rw-r--r--apps/drivers/stm32/adc/Makefile43
-rw-r--r--apps/drivers/stm32/adc/adc.cpp387
-rw-r--r--apps/px4io/adc.c163
-rw-r--r--apps/px4io/comms.c66
-rw-r--r--apps/px4io/protocol.h4
-rw-r--r--apps/px4io/px4io.h34
-rw-r--r--apps/sdlog/sdlog.c2
-rw-r--r--apps/sensors/sensors.cpp33
-rw-r--r--apps/uORB/objects_common.cpp3
-rw-r--r--apps/uORB/topics/battery_status.h68
-rw-r--r--apps/uORB/topics/sensor_combined.h4
-rw-r--r--nuttx/configs/px4fmu/nsh/appconfig1
-rwxr-xr-xnuttx/configs/px4fmu/nsh/defconfig26
-rwxr-xr-xnuttx/configs/px4io/io/defconfig1
21 files changed, 892 insertions, 221 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index ddbfc19d6..f7e93db6c 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -69,6 +69,7 @@
#include <poll.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/battery_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
@@ -1315,8 +1316,6 @@ int commander_thread_main(int argc, char *argv[])
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
struct sensor_combined_s sensors;
memset(&sensors, 0, sizeof(sensors));
- sensors.battery_voltage_v = 0.0f;
- sensors.battery_voltage_valid = false;
/* Subscribe to command topic */
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -1328,6 +1327,12 @@ int commander_thread_main(int argc, char *argv[])
struct parameter_update_s param_changed;
memset(&param_changed, 0, sizeof(param_changed));
+ /* subscribe to battery topic */
+ int battery_sub = orb_subscribe(ORB_ID(battery_status));
+ struct battery_status_s battery;
+ memset(&battery, 0, sizeof(battery));
+ battery.voltage_v = 0.0f;
+
// uint8_t vehicle_state_previous = current_status.state_machine;
float voltage_previous = 0.0f;
@@ -1413,15 +1418,19 @@ int commander_thread_main(int argc, char *argv[])
last_local_position_time = local_position.timestamp;
}
- battery_voltage = sensors.battery_voltage_v;
- battery_voltage_valid = sensors.battery_voltage_valid;
-
- /*
- * Only update battery voltage estimate if voltage is
- * valid and system has been running for two and a half seconds
- */
- if (battery_voltage_valid && (hrt_absolute_time() - start_time > 2500000)) {
- bat_remain = battery_remaining_estimate_voltage(battery_voltage);
+ orb_check(battery_sub, &new_data);
+ if (new_data) {
+ orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+ battery_voltage = battery.voltage_v;
+ battery_voltage_valid = true;
+
+ /*
+ * Only update battery voltage estimate if system has
+ * been running for two and a half seconds.
+ */
+ if (hrt_absolute_time() - start_time > 2500000) {
+ bat_remain = battery_remaining_estimate_voltage(battery_voltage);
+ }
}
/* Slow but important 8 Hz checks */
diff --git a/apps/drivers/boards/px4fmu/px4fmu_adc.c b/apps/drivers/boards/px4fmu/px4fmu_adc.c
deleted file mode 100644
index b55af486d..000000000
--- a/apps/drivers/boards/px4fmu/px4fmu_adc.c
+++ /dev/null
@@ -1,139 +0,0 @@
-/****************************************************************************
- *
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file px4fmu_adc.c
- *
- * Board-specific ADC functions.
- */
-
-/************************************************************************************
- * Included Files
- ************************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <errno.h>
-#include <debug.h>
-#include <stdio.h>
-
-#include <nuttx/analog/adc.h>
-#include <arch/board/board.h>
-
-#include "chip.h"
-#include "up_arch.h"
-
-#include "stm32_adc.h"
-#include "px4fmu_internal.h"
-
-#define ADC3_NCHANNELS 4
-
-/************************************************************************************
- * Private Data
- ************************************************************************************/
-/* The PX4FMU board has four ADC channels: ADC323 IN10-13
- */
-
-/* Identifying number of each ADC channel: Variable Resistor. */
-
-#ifdef CONFIG_STM32_ADC3
-static const uint8_t g_chanlist[ADC3_NCHANNELS] = {10, 11, 12, 13};
-
-/* Configurations of pins used byte each ADC channels */
-static const uint32_t g_pinlist[ADC3_NCHANNELS] = {GPIO_ADC3_IN10, GPIO_ADC3_IN11, GPIO_ADC3_IN12, GPIO_ADC3_IN13};
-#endif
-
-/************************************************************************************
- * Private Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Public Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Name: adc_devinit
- *
- * Description:
- * All STM32 architectures must provide the following interface to work with
- * examples/adc.
- *
- ************************************************************************************/
-
-int adc_devinit(void)
-{
- static bool initialized = false;
- struct adc_dev_s *adc[ADC3_NCHANNELS];
- int ret;
- int i;
-
- /* Check if we have already initialized */
-
- if (!initialized) {
- char name[11];
-
- for (i = 0; i < ADC3_NCHANNELS; i++) {
- stm32_configgpio(g_pinlist[i]);
- }
-
- for (i = 0; i < 1; i++) {
- /* Configure the pins as analog inputs for the selected channels */
- //stm32_configgpio(g_pinlist[i]);
-
- /* Call stm32_adcinitialize() to get an instance of the ADC interface */
- //multiple channels only supported with dma!
- adc[i] = stm32_adcinitialize(3, (g_chanlist), 4);
-
- if (adc == NULL) {
- adbg("ERROR: Failed to get ADC interface\n");
- return -ENODEV;
- }
-
-
- /* Register the ADC driver at "/dev/adc0" */
- sprintf(name, "/dev/adc%d", i);
- ret = adc_register(name, adc[i]);
-
- if (ret < 0) {
- adbg("adc_register failed for adc %s: %d\n", name, ret);
- return ret;
- }
- }
-
- /* Now we are initialized */
-
- initialized = true;
- }
-
- return OK;
-}
diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c
index bc047aa4f..2a7bfe3d7 100644
--- a/apps/drivers/boards/px4fmu/px4fmu_init.c
+++ b/apps/drivers/boards/px4fmu/px4fmu_init.c
@@ -95,8 +95,6 @@
* Protected Functions
****************************************************************************/
-extern int adc_devinit(void);
-
/****************************************************************************
* Public Functions
****************************************************************************/
@@ -201,7 +199,7 @@ __EXPORT int nsh_archinitialize(void)
message("[boot] Successfully initialized SPI port 1\r\n");
- /* Get the SPI port for the microsd slot */
+ /* Get the SPI port for the microSD slot */
message("[boot] Initializing SPI port 3\n");
spi3 = up_spiinitialize(3);
@@ -225,21 +223,10 @@ __EXPORT int nsh_archinitialize(void)
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
-#ifdef CONFIG_ADC
- int adc_state = adc_devinit();
-
- if (adc_state != OK) {
- /* Try again */
- adc_state = adc_devinit();
-
- if (adc_state != OK) {
- /* Give up */
- message("[boot] FAILED adc_devinit: %d\n", adc_state);
- return -ENODEV;
- }
- }
-
-#endif
+ stm32_configgpio(GPIO_ADC1_IN10);
+ stm32_configgpio(GPIO_ADC1_IN11);
+ stm32_configgpio(GPIO_ADC1_IN12);
+ //stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
return OK;
}
diff --git a/apps/drivers/boards/px4io/px4io_init.c b/apps/drivers/boards/px4io/px4io_init.c
index 33a6707ce..14e8dc13a 100644
--- a/apps/drivers/boards/px4io/px4io_init.c
+++ b/apps/drivers/boards/px4io/px4io_init.c
@@ -92,4 +92,7 @@ __EXPORT void stm32_boardinitialize(void)
stm32_configgpio(GPIO_ACC_OC_DETECT);
stm32_configgpio(GPIO_SERVO_OC_DETECT);
stm32_configgpio(GPIO_BTN_SAFETY);
+
+ stm32_configgpio(GPIO_ADC_VBATT);
+ stm32_configgpio(GPIO_ADC_IN5);
}
diff --git a/apps/drivers/boards/px4io/px4io_internal.h b/apps/drivers/boards/px4io/px4io_internal.h
index 3ac8a5cfa..f77d237a7 100644
--- a/apps/drivers/boards/px4io/px4io_internal.h
+++ b/apps/drivers/boards/px4io/px4io_internal.h
@@ -76,3 +76,8 @@
#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN13)
#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
+
+/* Analog inputs ********************************************************************/
+
+#define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
+#define GPIO_ADC_IN5 (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)
diff --git a/apps/drivers/drv_adc.h b/apps/drivers/drv_adc.h
new file mode 100644
index 000000000..8ec6d1233
--- /dev/null
+++ b/apps/drivers/drv_adc.h
@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_adc.h
+ *
+ * ADC driver interface.
+ *
+ * This defines additional operations over and above the standard NuttX
+ * ADC API.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#define ADC_DEVICE_PATH "/dev/adc0"
+
+/*
+ * ioctl definitions
+ */
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index e6a485de4..0c0b08343 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -78,6 +78,7 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/rc_channels.h>
+#include <uORB/topics/battery_status.h>
#include <px4io/protocol.h>
#include "uploader.h"
@@ -127,6 +128,9 @@ private:
orb_advert_t _to_input_rc; ///< rc inputs from io
rc_input_values _input_rc; ///< rc input values
+ orb_advert_t _to_battery; ///< battery status / voltage
+ battery_status_s _battery_status;///< battery status data
+
orb_advert_t _t_outputs; ///< mixed outputs topic
actuator_outputs_s _outputs; ///< mixed outputs
@@ -379,6 +383,10 @@ PX4IO::task_main()
memset(&_input_rc, 0, sizeof(_input_rc));
_to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc);
+ /* do not advertise the battery status until its clear that a battery is connected */
+ memset(&_battery_status, 0, sizeof(_battery_status));
+ _to_battery = -1;
+
/* poll descriptor */
pollfd fds[4];
fds[0].fd = _serial_fd;
@@ -559,6 +567,23 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
/* remember the latched arming switch state */
_switch_armed = rep->armed;
+ /* publish battery information */
+
+ /* only publish if battery has a valid minimum voltage */
+ if (rep->battery_mv > 3300) {
+ _battery_status.timestamp = hrt_absolute_time();
+ _battery_status.voltage_v = rep->battery_mv / 1000.0f;
+ /* current and discharge are unknown */
+ _battery_status.current_a = -1.0f;
+ _battery_status.discharged_mah = -1.0f;
+ /* announce the battery voltage if needed, just publish else */
+ if (_to_battery > 0) {
+ orb_publish(ORB_ID(battery_status), _to_battery, &_battery_status);
+ } else {
+ _to_battery = orb_advertise(ORB_ID(battery_status), &_battery_status);
+ }
+ }
+
_send_needed = true;
/* if monitoring, dump the received info */
diff --git a/apps/drivers/stm32/adc/Makefile b/apps/drivers/stm32/adc/Makefile
new file mode 100644
index 000000000..443bc0623
--- /dev/null
+++ b/apps/drivers/stm32/adc/Makefile
@@ -0,0 +1,43 @@
+############################################################################
+#
+# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# STM32 ADC driver
+#
+
+APPNAME = adc
+PRIORITY = SCHED_PRIORITY_DEFAULT
+STACKSIZE = 2048
+INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/stm32/adc/adc.cpp b/apps/drivers/stm32/adc/adc.cpp
new file mode 100644
index 000000000..87dac1ef9
--- /dev/null
+++ b/apps/drivers/stm32/adc/adc.cpp
@@ -0,0 +1,387 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file adc.cpp
+ *
+ * Driver for the STM32 ADC.
+ *
+ * This is a low-rate driver, designed for sampling things like voltages
+ * and so forth. It avoids the gross complexity of the NuttX ADC driver.
+ */
+
+#include <nuttx/config.h>
+#include <drivers/device/device.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <stdio.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_adc.h>
+
+#include <arch/stm32/chip.h>
+#include <stm32_internal.h>
+#include <stm32_gpio.h>
+
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+
+/*
+ * Register accessors.
+ * For now, no reason not to just use ADC1.
+ */
+#define REG(_reg) (*(volatile uint32_t *)(STM32_ADC1_BASE + _reg))
+
+#define rSR REG(STM32_ADC_SR_OFFSET)
+#define rCR1 REG(STM32_ADC_CR1_OFFSET)
+#define rCR2 REG(STM32_ADC_CR2_OFFSET)
+#define rSMPR1 REG(STM32_ADC_SMPR1_OFFSET)
+#define rSMPR2 REG(STM32_ADC_SMPR2_OFFSET)
+#define rJOFR1 REG(STM32_ADC_JOFR1_OFFSET)
+#define rJOFR2 REG(STM32_ADC_JOFR2_OFFSET)
+#define rJOFR3 REG(STM32_ADC_JOFR3_OFFSET)
+#define rJOFR4 REG(STM32_ADC_JOFR4_OFFSET)
+#define rHTR REG(STM32_ADC_HTR_OFFSET)
+#define rLTR REG(STM32_ADC_LTR_OFFSET)
+#define rSQR1 REG(STM32_ADC_SQR1_OFFSET)
+#define rSQR2 REG(STM32_ADC_SQR2_OFFSET)
+#define rSQR3 REG(STM32_ADC_SQR3_OFFSET)
+#define rJSQR REG(STM32_ADC_JSQR_OFFSET)
+#define rJDR1 REG(STM32_ADC_JDR1_OFFSET)
+#define rJDR2 REG(STM32_ADC_JDR2_OFFSET)
+#define rJDR3 REG(STM32_ADC_JDR3_OFFSET)
+#define rJDR4 REG(STM32_ADC_JDR4_OFFSET)
+#define rDR REG(STM32_ADC_DR_OFFSET)
+
+#ifdef STM32_ADC_CCR
+# define rCCR REG(STM32_ADC_CCR_OFFSET)
+#endif
+
+class ADC : public device::CDev
+{
+public:
+ ADC(uint32_t channels);
+ ~ADC();
+
+ virtual int init();
+
+ virtual int ioctl(file *filp, int cmd, unsigned long arg);
+ virtual ssize_t read(file *filp, char *buffer, size_t len);
+
+protected:
+ virtual int open_first(struct file *filp);
+ virtual int close_last(struct file *filp);
+
+private:
+ static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */
+
+ hrt_call _call;
+ perf_counter_t _sample_perf;
+
+ unsigned _channel_count;
+ adc_msg_s *_samples; /**< sample buffer */
+
+ /** work trampoline */
+ static void _tick_trampoline(void *arg);
+
+ /** worker function */
+ void _tick();
+
+ /**
+ * Sample a single channel and return the measured value.
+ *
+ * @param channel The channel to sample.
+ * @return The sampled value, or 0xffff if
+ * sampling failed.
+ */
+ uint16_t _sample(unsigned channel);
+
+};
+
+ADC::ADC(uint32_t channels) :
+ CDev("adc", ADC_DEVICE_PATH),
+ _sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
+ _channel_count(0),
+ _samples(nullptr)
+{
+ _debug_enabled = true;
+
+ /* always enable the temperature sensor */
+ channels |= 1 << 16;
+
+ /* allocate the sample array */
+ for (unsigned i = 0; i < 32; i++) {
+ if (channels & (1 << i)) {
+ _channel_count++;
+ }
+ }
+ _samples = new adc_msg_s[_channel_count];
+
+ /* prefill the channel numbers in the sample array */
+ if (_samples != nullptr) {
+ unsigned index = 0;
+ for (unsigned i = 0; i < 32; i++) {
+ if (channels & (1 << i)) {
+ _samples[index].am_channel = i;
+ _samples[index].am_data = 0;
+ index++;
+ }
+ }
+ }
+}
+
+ADC::~ADC()
+{
+ if (_samples != nullptr)
+ delete _samples;
+}
+
+int
+ADC::init()
+{
+ /* do calibration if supported */
+#ifdef ADC_CR2_CAL
+ rCR2 |= ADC_CR2_CAL;
+ usleep(100);
+ if (rCR2 & ADC_CR2_CAL)
+ return -1;
+#endif
+
+ /* arbitrarily configure all channels for 55 cycle sample time */
+ rSMPR1 = 0b00000011011011011011011011011011;
+ rSMPR2 = 0b00011011011011011011011011011011;
+
+ /* XXX for F2/4, might want to select 12-bit mode? */
+ rCR1 = 0;
+
+ /* enable the temperature sensor / Vrefint channel if supported*/
+ rCR2 =
+#ifdef ADC_CR2_TSVREFE
+ /* enable the temperature sensor in CR2 */
+ ADC_CR2_TSVREFE |
+#endif
+ 0;
+
+#ifdef ADC_CCR_TSVREFE
+ /* enable temperature sensor in CCR */
+ rCCR = ADC_CCR_TSVREFE;
+#endif
+
+ /* configure for a single-channel sequence */
+ rSQR1 = 0;
+ rSQR2 = 0;
+ rSQR3 = 0; /* will be updated with the channel each tick */
+
+ /* power-cycle the ADC and turn it on */
+ rCR2 &= ~ADC_CR2_ADON;
+ usleep(10);
+ rCR2 |= ADC_CR2_ADON;
+ usleep(10);
+ rCR2 |= ADC_CR2_ADON;
+ usleep(10);
+
+ /* kick off a sample and wait for it to complete */
+ hrt_abstime now = hrt_absolute_time();
+ rCR2 |= ADC_CR2_SWSTART;
+ while (!(rSR & ADC_SR_EOC)) {
+
+ /* don't wait for more than 500us, since that means something broke - should reset here if we see this */
+ if ((hrt_absolute_time() - now) > 500) {
+ log("sample timeout");
+ return -1;
+ return 0xffff;
+ }
+ }
+
+
+ debug("init done");
+
+ /* create the device node */
+ return CDev::init();
+}
+
+int
+ADC::ioctl(file *filp, int cmd, unsigned long arg)
+{
+ return -ENOTTY;
+}
+
+ssize_t
+ADC::read(file *filp, char *buffer, size_t len)
+{
+ const size_t maxsize = sizeof(adc_msg_s) * _channel_count;
+
+ if (len > maxsize)
+ len = maxsize;
+
+ /* block interrupts while copying samples to avoid racing with an update */
+ irqstate_t flags = irqsave();
+ memcpy(buffer, _samples, len);
+ irqrestore(flags);
+
+ return len;
+}
+
+int
+ADC::open_first(struct file *filp)
+{
+ /* get fresh data */
+ _tick();
+
+ /* and schedule regular updates */
+ hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this);
+
+ return 0;
+}
+
+int
+ADC::close_last(struct file *filp)
+{
+ hrt_cancel(&_call);
+ return 0;
+}
+
+void
+ADC::_tick_trampoline(void *arg)
+{
+ ((ADC *)arg)->_tick();
+}
+
+void
+ADC::_tick()
+{
+ /* scan the channel set and sample each */
+ for (unsigned i = 0; i < _channel_count; i++)
+ _samples[i].am_data = _sample(_samples[i].am_channel);
+}
+
+uint16_t
+ADC::_sample(unsigned channel)
+{
+ perf_begin(_sample_perf);
+
+ /* clear any previous EOC */
+ if (rSR & ADC_SR_EOC)
+ rSR &= ~ADC_SR_EOC;
+
+ /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
+ rSQR3 = channel;
+ rCR2 |= ADC_CR2_SWSTART;
+
+ /* wait for the conversion to complete */
+ hrt_abstime now = hrt_absolute_time();
+ while (!(rSR & ADC_SR_EOC)) {
+
+ /* don't wait for more than 50us, since that means something broke - should reset here if we see this */
+ if ((hrt_absolute_time() - now) > 50) {
+ log("sample timeout");
+ return 0xffff;
+ }
+ }
+
+ /* read the result and clear EOC */
+ uint16_t result = rDR;
+
+ perf_end(_sample_perf);
+ return result;
+}
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int adc_main(int argc, char *argv[]);
+
+namespace
+{
+ADC *g_adc;
+
+void
+test(void)
+{
+
+ int fd = open(ADC_DEVICE_PATH, O_RDONLY);
+ if (fd < 0)
+ err(1, "can't open ADC device");
+
+ for (unsigned i = 0; i < 50; i++) {
+ adc_msg_s data[8];
+ ssize_t count = read(fd, data, sizeof(data));
+
+ if (count < 0)
+ errx(1, "read error");
+
+ unsigned channels = count / sizeof(data[0]);
+
+ for (unsigned j = 0; j < channels; j++) {
+ printf ("%d: %u ", data[j].am_channel, data[j].am_data);
+ }
+
+ printf("\n");
+ usleep(500000);
+ }
+
+ exit(0);
+}
+}
+
+int
+adc_main(int argc, char *argv[])
+{
+ if (g_adc == nullptr) {
+ /* XXX this hardcodes the minimum channel set for PX4FMU - should be configurable */
+ g_adc = new ADC((1 << 10) | (1 << 11));
+
+ if (g_adc == nullptr)
+ errx(1, "couldn't allocate the ADC driver");
+
+ if (g_adc->init() != OK) {
+ delete g_adc;
+ errx(1, "ADC init failed");
+ }
+ }
+
+ if (argc > 1) {
+ if (!strcmp(argv[1], "test"))
+ test();
+ }
+
+ exit(0);
+}
diff --git a/apps/px4io/adc.c b/apps/px4io/adc.c
new file mode 100644
index 000000000..62ff0b1f1
--- /dev/null
+++ b/apps/px4io/adc.c
@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file adc.c
+ *
+ * Simple ADC support for PX4IO on STM32.
+ */
+#include <nuttx/config.h>
+#include <stdint.h>
+
+#include <nuttx/arch.h>
+#include <arch/stm32/chip.h>
+#include <stm32_internal.h>
+
+#include <drivers/drv_hrt.h>
+#include <systemlib/perf_counter.h>
+
+#define DEBUG
+#include "px4io.h"
+
+/*
+ * Register accessors.
+ * For now, no reason not to just use ADC1.
+ */
+#define REG(_reg) (*(volatile uint32_t *)(STM32_ADC1_BASE + _reg))
+
+#define rSR REG(STM32_ADC_SR_OFFSET)
+#define rCR1 REG(STM32_ADC_CR1_OFFSET)
+#define rCR2 REG(STM32_ADC_CR2_OFFSET)
+#define rSMPR1 REG(STM32_ADC_SMPR1_OFFSET)
+#define rSMPR2 REG(STM32_ADC_SMPR2_OFFSET)
+#define rJOFR1 REG(STM32_ADC_JOFR1_OFFSET)
+#define rJOFR2 REG(STM32_ADC_JOFR2_OFFSET)
+#define rJOFR3 REG(STM32_ADC_JOFR3_OFFSET)
+#define rJOFR4 REG(STM32_ADC_JOFR4_OFFSET)
+#define rHTR REG(STM32_ADC_HTR_OFFSET)
+#define rLTR REG(STM32_ADC_LTR_OFFSET)
+#define rSQR1 REG(STM32_ADC_SQR1_OFFSET)
+#define rSQR2 REG(STM32_ADC_SQR2_OFFSET)
+#define rSQR3 REG(STM32_ADC_SQR3_OFFSET)
+#define rJSQR REG(STM32_ADC_JSQR_OFFSET)
+#define rJDR1 REG(STM32_ADC_JDR1_OFFSET)
+#define rJDR2 REG(STM32_ADC_JDR2_OFFSET)
+#define rJDR3 REG(STM32_ADC_JDR3_OFFSET)
+#define rJDR4 REG(STM32_ADC_JDR4_OFFSET)
+#define rDR REG(STM32_ADC_DR_OFFSET)
+
+perf_counter_t adc_perf;
+
+int
+adc_init(void)
+{
+ adc_perf = perf_alloc(PC_ELAPSED, "adc");
+
+ /* do calibration if supported */
+#ifdef ADC_CR2_CAL
+ rCR2 |= ADC_CR2_RSTCAL;
+ up_udelay(1);
+ if (rCR2 & ADC_CR2_RSTCAL)
+ return -1;
+ rCR2 |= ADC_CR2_CAL;
+ up_udelay(100);
+ if (rCR2 & ADC_CR2_CAL)
+ return -1;
+#endif
+
+ /* arbitrarily configure all channels for 55 cycle sample time */
+ rSMPR1 = 0b00000011011011011011011011011011;
+ rSMPR2 = 0b00011011011011011011011011011011;
+
+ /* XXX for F2/4, might want to select 12-bit mode? */
+ rCR1 = 0;
+
+ /* enable the temperature sensor / Vrefint channel if supported*/
+ rCR2 =
+#ifdef ADC_CR2_TSVREFE
+ /* enable the temperature sensor in CR2 */
+ ADC_CR2_TSVREFE |
+#endif
+ 0;
+
+#ifdef ADC_CCR_TSVREFE
+ /* enable temperature sensor in CCR */
+ rCCR = ADC_CCR_TSVREFE;
+#endif
+
+ /* configure for a single-channel sequence */
+ rSQR1 = 0;
+ rSQR2 = 0;
+ rSQR3 = 0; /* will be updated with the channel each tick */
+
+ /* power-cycle the ADC and turn it on */
+ rCR2 &= ~ADC_CR2_ADON;
+ up_udelay(10);
+ rCR2 |= ADC_CR2_ADON;
+ up_udelay(10);
+ rCR2 |= ADC_CR2_ADON;
+ up_udelay(10);
+
+ return 0;
+}
+
+uint16_t
+adc_measure(unsigned channel)
+{
+ perf_begin(adc_perf);
+
+ /* clear any previous EOC */
+ if (rSR & ADC_SR_EOC)
+ rSR &= ~ADC_SR_EOC;
+
+ /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
+ rSQR3 = channel;
+ rCR2 |= ADC_CR2_ADON;
+
+ /* wait for the conversion to complete */
+ hrt_abstime now = hrt_absolute_time();
+ while (!(rSR & ADC_SR_EOC)) {
+
+ /* never spin forever - this will give a bogus result though */
+ if ((hrt_absolute_time() - now) > 1000) {
+ debug("adc timeout");
+ break;
+ }
+ }
+
+ /* read the result and clear EOC */
+ uint16_t result = rDR;
+
+ perf_end(adc_perf);
+ return result;
+} \ No newline at end of file
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index b22b09183..8d7e90093 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -62,8 +62,7 @@
#define FMU_MIN_REPORT_INTERVAL 5000 /* 5ms */
#define FMU_MAX_REPORT_INTERVAL 100000 /* 100ms */
-int frame_rx;
-int frame_bad;
+#define FMU_STATUS_INTERVAL 1000000 /* 100ms */
static int fmu_fd;
static hx_stream_t stream;
@@ -93,6 +92,9 @@ comms_init(void)
cfsetspeed(&t, 115200);
t.c_cflag &= ~(CSTOPB | PARENB);
tcsetattr(fmu_fd, TCSANOW, &t);
+
+ /* init the ADC */
+ adc_init();
}
void
@@ -145,9 +147,55 @@ comms_main(void)
report.channel_count = system_state.rc_channels;
report.armed = system_state.armed;
+ report.battery_mv = system_state.battery_mv;
+ report.adc_in = system_state.adc_in5;
+ report.overcurrent = system_state.overcurrent;
+
/* and send it */
hx_stream_send(stream, &report, sizeof(report));
}
+
+ /*
+ * Fetch ADC values, check overcurrent flags, etc.
+ */
+ static hrt_abstime last_status_time;
+
+ if ((now - last_status_time) > FMU_STATUS_INTERVAL) {
+
+ /*
+ * Coefficients here derived by measurement of the 5-16V
+ * range on one unit:
+ *
+ * V counts
+ * 5 1001
+ * 6 1219
+ * 7 1436
+ * 8 1653
+ * 9 1870
+ * 10 2086
+ * 11 2303
+ * 12 2522
+ * 13 2738
+ * 14 2956
+ * 15 3172
+ * 16 3389
+ *
+ * slope = 0.0046067
+ * intercept = 0.3863
+ *
+ * Intercept corrected for best results @ 12V.
+ */
+ unsigned counts = adc_measure(ADC_VBATT);
+ system_state.battery_mv = (4150 + (counts * 46)) / 10;
+
+ system_state.adc_in5 = adc_measure(ADC_IN5);
+
+ system_state.overcurrent =
+ (OVERCURRENT_SERVO ? (1 << 0) : 0) |
+ (OVERCURRENT_ACC ? (1 << 1) : 0);
+
+ last_status_time = now;
+ }
}
}
@@ -156,12 +204,10 @@ comms_handle_config(const void *buffer, size_t length)
{
const struct px4io_config *cfg = (struct px4io_config *)buffer;
- if (length != sizeof(*cfg)) {
- frame_bad++;
+ if (length != sizeof(*cfg))
return;
- }
- frame_rx++;
+ /* XXX */
}
static void
@@ -169,12 +215,9 @@ comms_handle_command(const void *buffer, size_t length)
{
const struct px4io_command *cmd = (struct px4io_command *)buffer;
- if (length != sizeof(*cmd)) {
- frame_bad++;
+ if (length != sizeof(*cmd))
return;
- }
-
- frame_rx++;
+
irqstate_t flags = irqsave();
/* fetch new PWM output values */
@@ -261,7 +304,6 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
break;
default:
- frame_bad++;
break;
}
}
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 3a049bb29..163c52c02 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -72,6 +72,10 @@ struct px4io_report {
uint16_t rc_channel[PX4IO_INPUT_CHANNELS];
bool armed;
uint8_t channel_count;
+
+ uint16_t battery_mv;
+ uint16_t adc_in;
+ uint8_t overcurrent;
};
/**
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 782d9bfeb..157135854 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -139,13 +139,27 @@ struct sys_state_s {
* If true, IO performs an on-board manual override with the RC channel values
*/
bool manual_override_ok;
+
+ /*
+ * Measured battery voltage in mV
+ */
+ uint16_t battery_mv;
+
+ /*
+ * ADC IN5 measurement
+ */
+ uint16_t adc_in5;
+
+ /*
+ * Power supply overcurrent status bits.
+ */
+ uint8_t overcurrent;
+
};
extern struct sys_state_s system_state;
-extern int frame_rx;
-extern int frame_bad;
-
+#if 0
/*
* Software countdown timers.
*
@@ -157,6 +171,7 @@ extern int frame_bad;
#define TIMER_SANITY 7
#define TIMER_NUM_TIMERS 8
extern volatile int timers[TIMER_NUM_TIMERS];
+#endif
/*
* GPIO handling.
@@ -171,10 +186,13 @@ extern volatile int timers[TIMER_NUM_TIMERS];
#define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
#define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
-#define OVERCURRENT_ACC stm32_gpioread(GPIO_ACC_OC_DETECT)
-#define OVERCURRENT_SERVO stm32_gpioread(GPIO_SERVO_OC_DETECT
+#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT))
+#define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT))
#define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY)
+#define ADC_VBATT 4
+#define ADC_IN5 5
+
/*
* Mixer
*/
@@ -192,6 +210,12 @@ extern void safety_init(void);
extern void comms_main(void) __attribute__((noreturn));
/*
+ * Sensors/misc inputs
+ */
+extern int adc_init(void);
+extern uint16_t adc_measure(unsigned channel);
+
+/*
* R/C receiver handling.
*/
extern void controls_main(void);
diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c
index d38bf9122..9b4cd1622 100644
--- a/apps/sdlog/sdlog.c
+++ b/apps/sdlog/sdlog.c
@@ -554,7 +554,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
.control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]},
.actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3],
buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]},
- .vbat = buf.raw.battery_voltage_v,
+ .vbat = 0.0f, /* XXX use battery_status uORB topic */
.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index c331ec3e3..c0195df4f 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -73,6 +73,7 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/battery_status.h>
#define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */
#define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */
@@ -158,10 +159,12 @@ private:
orb_advert_t _sensor_pub; /**< combined sensor data topic */
orb_advert_t _manual_control_pub; /**< manual control signal topic */
orb_advert_t _rc_pub; /**< raw r/c control topic */
+ orb_advert_t _battery_pub; /**< battery status */
perf_counter_t _loop_perf; /**< loop performance counter */
struct rc_channels_s _rc; /**< r/c channel data */
+ struct battery_status_s _battery_status; /**< battery status */
struct {
float min[_rc_max_chan_count];
@@ -384,6 +387,7 @@ Sensors::Sensors() :
_sensor_pub(-1),
_manual_control_pub(-1),
_rc_pub(-1),
+ _battery_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
@@ -964,14 +968,22 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) {
/* Voltage in volts */
- raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (buf_adc.am_data1 * _parameters.battery_voltage_scaling)));
+ float voltage = (buf_adc.am_data1 * _parameters.battery_voltage_scaling);
- if ((raw.battery_voltage_v) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
- raw.battery_voltage_valid = false;
- raw.battery_voltage_v = 0.f;
+ if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
- } else {
- raw.battery_voltage_valid = true;
+ _battery_status.timestamp = hrt_absolute_time();
+ _battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));;
+ /* current and discharge are unknown */
+ _battery_status.current_a = -1.0f;
+ _battery_status.discharged_mah = -1.0f;
+
+ /* announce the battery voltage if needed, just publish else */
+ if (_battery_pub > 0) {
+ orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
+ } else {
+ _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
+ }
}
raw.battery_voltage_counter++;
@@ -999,7 +1011,7 @@ Sensors::ppm_poll()
*/
if (ppm_decoded_channels > 4 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) {
- for (unsigned int i = 0; i < ppm_decoded_channels; i++) {
+ for (unsigned i = 0; i < ppm_decoded_channels; i++) {
raw.values[i] = ppm_buffer[i];
}
@@ -1229,12 +1241,13 @@ Sensors::task_main()
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
raw.timestamp = hrt_absolute_time();
- raw.battery_voltage_v = BAT_VOL_INITIAL;
raw.adc_voltage_v[0] = 0.0f;
raw.adc_voltage_v[1] = 0.0f;
raw.adc_voltage_v[2] = 0.0f;
- raw.battery_voltage_counter = 0;
- raw.battery_voltage_valid = false;
+ raw.adc_voltage_v[3] = 0.0f;
+
+ memset(&_battery_status, 0, sizeof(_battery_status));
+ _battery_status.voltage_v = BAT_VOL_INITIAL;
/* get a set of initial values */
accel_poll(raw);
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index dbee15050..2d249a47f 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -71,6 +71,9 @@ ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s);
#include "topics/vehicle_status.h"
ORB_DEFINE(vehicle_status, struct vehicle_status_s);
+#include "topics/battery_status.h"
+ORB_DEFINE(battery_status, struct battery_status_s);
+
#include "topics/vehicle_global_position.h"
ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
diff --git a/apps/uORB/topics/battery_status.h b/apps/uORB/topics/battery_status.h
new file mode 100644
index 000000000..c40d0d4e5
--- /dev/null
+++ b/apps/uORB/topics/battery_status.h
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file battery_status.h
+ *
+ * Definition of the battery status uORB topic.
+ */
+
+#ifndef BATTERY_STATUS_H_
+#define BATTERY_STATUS_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Battery voltages and status
+ */
+struct battery_status_s {
+ uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
+ float voltage_v; /**< Battery voltage in volts, filtered */
+ float current_a; /**< Battery current in amperes, filtered, -1 if unknown */
+ float discharged_mah; /**< Discharged amount in mAh, filtered, -1 if unknown */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(battery_status);
+
+#endif \ No newline at end of file
diff --git a/apps/uORB/topics/sensor_combined.h b/apps/uORB/topics/sensor_combined.h
index 0324500ac..1d25af35a 100644
--- a/apps/uORB/topics/sensor_combined.h
+++ b/apps/uORB/topics/sensor_combined.h
@@ -99,8 +99,8 @@ struct sensor_combined_s {
float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */
float baro_alt_meter; /**< Altitude, already temp. comp. */
float baro_temp_celcius; /**< Temperature in degrees celsius */
- float battery_voltage_v; /**< Battery voltage in volts, filtered */
- float adc_voltage_v[3]; /**< ADC voltages of ADC Chan 11/12/13 or -1 */
+ float adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */
+ float mcu_temp_celcius; /**< Internal temperature measurement of MCU */
uint32_t baro_counter; /**< Number of raw baro measurements taken */
uint32_t battery_voltage_counter; /**< Number of voltage measurements taken */
bool battery_voltage_valid; /**< True if battery voltage can be measured */
diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig
index 0959687de..7f1a93df9 100644
--- a/nuttx/configs/px4fmu/nsh/appconfig
+++ b/nuttx/configs/px4fmu/nsh/appconfig
@@ -100,6 +100,7 @@ CONFIGURED_APPS += drivers/stm32
CONFIGURED_APPS += drivers/led
CONFIGURED_APPS += drivers/blinkm
CONFIGURED_APPS += drivers/stm32/tone_alarm
+CONFIGURED_APPS += drivers/stm32/adc
CONFIGURED_APPS += drivers/px4fmu
CONFIGURED_APPS += drivers/hil
diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig
index bc724c006..d9a48f2c8 100755
--- a/nuttx/configs/px4fmu/nsh/defconfig
+++ b/nuttx/configs/px4fmu/nsh/defconfig
@@ -189,9 +189,10 @@ CONFIG_STM32_TIM1=n
CONFIG_STM32_TIM8=n
CONFIG_STM32_USART1=y
CONFIG_STM32_USART6=y
-CONFIG_STM32_ADC1=n
+# We use our own driver, but leave this on.
+CONFIG_STM32_ADC1=y
CONFIG_STM32_ADC2=n
-CONFIG_STM32_ADC3=y
+CONFIG_STM32_ADC3=n
CONFIG_STM32_SDIO=n
CONFIG_STM32_SPI1=y
CONFIG_STM32_SYSCFG=y
@@ -359,27 +360,6 @@ CONFIG_STM32_I2CTIMEOUS_START_STOP=700
CONFIG_I2C_WRITEREAD=y
#
-# ADC configuration
-#
-# Enable ADC driver support.
-#
-# CONFIG_ADC=y : Enable the generic ADC infrastructure
-# CONFIG_STM32_TIM1_ADC=y : Indicate that timer 1 will be used to trigger an ADC
-# CONFIG_STM32_TIM1_ADC3=y : Assign timer 1 to drive ADC3 sampling
-# CONFIG_STM32_ADC3_SAMPLE_FREQUENCY=100 : Select a sampling frequency
-#
-CONFIG_ADC=y
-CONFIG_STM32_TIM4_ADC3=y
-# select sample frequency 1^=1.5Msamples/second
-# 65535^=10samples/second 16bit-timer runs at 84/128 MHz
-CONFIG_STM32_ADC3_SAMPLE_FREQUENCY=6000
-# select timer channel 0=CC1,...,3=CC4
-CONFIG_STM32_ADC3_TIMTRIG=3
-CONFIG_ADC_DMA=y
-# only 4 places usable!
-CONFIG_ADC_FIFOSIZE=5
-
-#
# General build options
#
# CONFIG_RRLOAD_BINARY - make the rrload binary format used with
diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig
index 74433e86c..30ec5be08 100755
--- a/nuttx/configs/px4io/io/defconfig
+++ b/nuttx/configs/px4io/io/defconfig
@@ -133,6 +133,7 @@ CONFIG_STM32_BKP=n
CONFIG_STM32_PWR=n
CONFIG_STM32_DAC=n
# APB2:
+# We use our own ADC driver, but leave this on for clocking purposes.
CONFIG_STM32_ADC1=y
CONFIG_STM32_ADC2=n
# TIM1 is owned by the HRT