forked from Archive/PX4-Autopilot
Merge pull request #130 from PX4/px4io-adc-integration-battery-msg
Enables the PX4IO ADC (from px4io-adc-integration branch)
This commit is contained in:
Lorenz Meier
commit
309980cd4d
|
|
@ -70,6 +70,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_gps_position.h>
|
||||
|
|
@ -1262,6 +1263,11 @@ int commander_thread_main(int argc, char *argv[])
|
|||
struct vehicle_command_s cmd;
|
||||
memset(&cmd, 0, sizeof(cmd));
|
||||
|
||||
/* subscribe to battery topic */
|
||||
int battery_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
struct battery_status_s battery;
|
||||
memset(&battery, 0, sizeof(battery));
|
||||
|
||||
// uint8_t vehicle_state_previous = current_status.state_machine;
|
||||
float voltage_previous = 0.0f;
|
||||
|
||||
|
|
@ -1300,15 +1306,19 @@ int commander_thread_main(int argc, char *argv[])
|
|||
handle_command(stat_pub, ¤t_status, &cmd);
|
||||
}
|
||||
|
||||
battery_voltage = sensors.battery_voltage_v;
|
||||
battery_voltage_valid = sensors.battery_voltage_valid;
|
||||
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 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);
|
||||
/*
|
||||
* 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 */
|
||||
|
|
|
|||
|
|
@ -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}; ADC12 and 13 are used by MPU on v1.5 boards
|
||||
|
||||
/* Configurations of pins used byte each ADC channels */
|
||||
static const uint32_t g_pinlist[ADC3_NCHANNELS] = {GPIO_ADC3_IN10, GPIO_ADC3_IN11}; // ADC12 and 13 are used by MPU on v1.5 boards, 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;
|
||||
}
|
||||
|
|
@ -95,8 +95,6 @@
|
|||
* Protected Functions
|
||||
****************************************************************************/
|
||||
|
||||
extern int adc_devinit(void);
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
|
@ -209,8 +207,7 @@ __EXPORT int nsh_archinitialize(void)
|
|||
|
||||
message("[boot] Successfully initialized SPI port 1\r\n");
|
||||
|
||||
#if defined(CONFIG_STM32_SPI3)
|
||||
/* Get the SPI port */
|
||||
/* Get the SPI port for the microSD slot */
|
||||
|
||||
message("[boot] Initializing SPI port 3\n");
|
||||
spi3 = up_spiinitialize(3);
|
||||
|
|
@ -233,23 +230,11 @@ __EXPORT int nsh_archinitialize(void)
|
|||
}
|
||||
|
||||
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
|
||||
#endif /* SPI3 */
|
||||
|
||||
#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); // XXX is this available?
|
||||
//stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
*/
|
||||
|
|
@ -74,6 +74,7 @@
|
|||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
|
||||
#include <px4io/protocol.h>
|
||||
#include "uploader.h"
|
||||
|
|
@ -110,6 +111,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
|
||||
|
||||
|
|
@ -339,6 +343,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[3];
|
||||
fds[0].fd = _serial_fd;
|
||||
|
|
@ -511,6 +519,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 */
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -61,8 +61,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;
|
||||
|
|
@ -92,6 +91,9 @@ comms_init(void)
|
|||
cfsetspeed(&t, 115200);
|
||||
t.c_cflag &= ~(CSTOPB | PARENB);
|
||||
tcsetattr(fmu_fd, TCSANOW, &t);
|
||||
|
||||
/* init the ADC */
|
||||
adc_init();
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -143,9 +145,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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -154,12 +202,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
|
||||
|
|
@ -167,12 +213,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 */
|
||||
|
|
@ -237,7 +280,6 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
|
|||
break;
|
||||
|
||||
default:
|
||||
frame_bad++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -69,6 +69,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;
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -109,17 +109,25 @@ struct sys_state_s {
|
|||
bool fmu_data_received;
|
||||
|
||||
/*
|
||||
* Current serial interface mode, per the serial_rx_mode parameter
|
||||
* in the config packet.
|
||||
* Measured battery voltage in mV
|
||||
*/
|
||||
uint8_t serial_rx_mode;
|
||||
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.
|
||||
*
|
||||
|
|
@ -131,6 +139,7 @@ extern int frame_bad;
|
|||
#define TIMER_SANITY 7
|
||||
#define TIMER_NUM_TIMERS 8
|
||||
extern volatile int timers[TIMER_NUM_TIMERS];
|
||||
#endif
|
||||
|
||||
/*
|
||||
* GPIO handling.
|
||||
|
|
@ -145,10 +154,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
|
||||
*/
|
||||
|
|
@ -165,6 +177,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.
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -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},
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
@ -156,10 +157,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];
|
||||
|
|
@ -348,6 +351,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"))
|
||||
|
|
@ -844,14 +848,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++;
|
||||
|
|
@ -879,7 +891,7 @@ Sensors::ppm_poll()
|
|||
*/
|
||||
if (ppm_decoded_channels > 4 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) {
|
||||
|
||||
for (int i = 0; i < ppm_decoded_channels; i++) {
|
||||
for (unsigned i = 0; i < ppm_decoded_channels; i++) {
|
||||
raw.values[i] = ppm_buffer[i];
|
||||
}
|
||||
|
||||
|
|
@ -1039,12 +1051,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.9f;
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -358,27 +359,6 @@ CONFIG_STM32_I2CTIMEOUS_START_STOP=700
|
|||
# XXX this is bad and we want it gone
|
||||
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
|
||||
#
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue