aboutsummaryrefslogtreecommitdiff
path: root/apps/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'apps/drivers')
-rw-r--r--apps/drivers/blinkm/blinkm.cpp443
-rw-r--r--apps/drivers/boards/px4fmu/px4fmu_adc.c139
-rw-r--r--apps/drivers/boards/px4fmu/px4fmu_init.c63
-rw-r--r--apps/drivers/boards/px4io/px4io_init.c3
-rw-r--r--apps/drivers/boards/px4io/px4io_internal.h27
-rw-r--r--apps/drivers/drv_adc.h52
-rw-r--r--apps/drivers/drv_mixer.h23
-rw-r--r--apps/drivers/drv_pwm_output.h3
-rw-r--r--apps/drivers/hil/hil.cpp58
-rw-r--r--apps/drivers/px4fmu/fmu.cpp77
-rw-r--r--apps/drivers/px4io/px4io.cpp412
-rw-r--r--apps/drivers/px4io/uploader.cpp132
-rw-r--r--apps/drivers/px4io/uploader.h9
-rw-r--r--apps/drivers/stm32/adc/Makefile43
-rw-r--r--apps/drivers/stm32/adc/adc.cpp387
15 files changed, 1483 insertions, 388 deletions
diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp
index d589025cc..aeee80905 100644
--- a/apps/drivers/blinkm/blinkm.cpp
+++ b/apps/drivers/blinkm/blinkm.cpp
@@ -35,9 +35,61 @@
* @file blinkm.cpp
*
* Driver for the BlinkM LED controller connected via I2C.
+ *
+ * Connect the BlinkM to I2C3 and put the following line to the rc startup-script:
+ * blinkm start
+ *
+ * To start the system monitor put in the next line after the blinkm start:
+ * blinkm systemmonitor
+ *
+ *
+ * Description:
+ * After startup, the Application checked how many lipo cells are connected to the System.
+ * The recognized number off cells, will be blinked 5 times in purple color.
+ * 2 Cells = 2 blinks
+ * ...
+ * 5 Cells = 5 blinks
+ * Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts.
+ *
+ * System disarmed:
+ * The BlinkM should lit solid red.
+ *
+ * System armed:
+ * One message is made of 4 Blinks and a pause in the same length as the 4 blinks.
+ *
+ * X-X-X-X-_-_-_-_-
+ * -------------------------
+ * G G G M
+ * P P P O
+ * S S S D
+ * E
+ *
+ * (X = on, _=off)
+ *
+ * The first 3 blinks indicates the status of the GPS-Signal (red):
+ * 0-4 satellites = X-X-X-X-_-_-_-_-
+ * 5 satellites = X-X-_-X-_-_-_-_-
+ * 6 satellites = X-_-_-X-_-_-_-_-
+ * >=7 satellites = _-_-_-X-_-_-_-_-
+ * If no GPS is found the first 3 blinks are white
+ *
+ * The fourth Blink indicates the Flightmode:
+ * MANUAL : off
+ * STABILIZED : yellow
+ * HOLD : blue
+ * AUTO : green
+ *
+ * Battery Warning (low Battery Level):
+ * Continuously blinking in yellow X-X-X-X-X-X-X-X
+ *
+ * Battery Alert (critical Battery Level)
+ * Continuously blinking in red X-X-X-X-X-X-X-X
+ *
+ * General Error (no uOrb Data)
+ * Continuously blinking in white X-X-X-X-X-X-X-X
+ *
*/
-
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
@@ -59,15 +111,28 @@
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+#include <poll.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+static const float MAX_CELL_VOLTAGE = 4.3f;
+static const int LED_ONTIME = 100;
+static const int LED_OFFTIME = 100;
+static const int LED_BLINK = 1;
+static const int LED_NOBLINK = 0;
+
class BlinkM : public device::I2C
{
public:
BlinkM(int bus);
~BlinkM();
+
virtual int init();
virtual int probe();
-
+ virtual int setMode(int mode);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
static const char *script_names[];
@@ -95,11 +160,31 @@ private:
MORSE_CODE
};
+ enum ledColors {
+ LED_OFF,
+ LED_RED,
+ LED_YELLOW,
+ LED_PURPLE,
+ LED_GREEN,
+ LED_BLUE,
+ LED_WHITE
+ };
+
work_s _work;
- static const unsigned _monitor_interval = 250;
- static void monitor_trampoline(void *arg);
- void monitor();
+ int led_color_1;
+ int led_color_2;
+ int led_color_3;
+ int led_color_4;
+ int led_color_5;
+ int led_color_6;
+ int led_blink;
+
+ bool systemstate_run;
+
+ void setLEDColor(int ledcolor);
+ static void led_trampoline(void *arg);
+ void led();
int set_rgb(uint8_t r, uint8_t g, uint8_t b);
@@ -127,8 +212,7 @@ private:
/* for now, we only support one BlinkM */
namespace
{
-BlinkM *g_blinkm;
-
+ BlinkM *g_blinkm;
}
/* list of script names, must match script ID numbers */
@@ -155,11 +239,21 @@ const char *BlinkM::script_names[] = {
nullptr
};
+
extern "C" __EXPORT int blinkm_main(int argc, char *argv[]);
BlinkM::BlinkM(int bus) :
- I2C("blinkm", BLINKM_DEVICE_PATH, bus, 0x09, 100000)
+ I2C("blinkm", BLINKM_DEVICE_PATH, bus, 0x09, 100000),
+ led_color_1(LED_OFF),
+ led_color_2(LED_OFF),
+ led_color_3(LED_OFF),
+ led_color_4(LED_OFF),
+ led_color_5(LED_OFF),
+ led_color_6(LED_OFF),
+ led_blink(LED_NOBLINK),
+ systemstate_run(false)
{
+ memset(&_work, 0, sizeof(_work));
}
BlinkM::~BlinkM()
@@ -170,7 +264,6 @@ int
BlinkM::init()
{
int ret;
-
ret = I2C::init();
if (ret != OK) {
@@ -178,14 +271,25 @@ BlinkM::init()
return ret;
}
- /* set some sensible defaults */
- set_fade_speed(25);
+ stop_script();
+ set_rgb(0,0,0);
- /* turn off by default */
- play_script(BLACK);
+ return OK;
+}
- /* start the system monitor as a low-priority workqueue entry */
- work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, 1);
+int
+BlinkM::setMode(int mode)
+{
+ if(mode == 1) {
+ if(systemstate_run == false) {
+ stop_script();
+ set_rgb(0,0,0);
+ systemstate_run = true;
+ work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1);
+ }
+ } else {
+ systemstate_run = false;
+ }
return OK;
}
@@ -249,21 +353,300 @@ BlinkM::ioctl(struct file *filp, int cmd, unsigned long arg)
return ret;
}
+
void
-BlinkM::monitor_trampoline(void *arg)
+BlinkM::led_trampoline(void *arg)
{
BlinkM *bm = (BlinkM *)arg;
- bm->monitor();
+ bm->led();
}
+
+
void
-BlinkM::monitor()
+BlinkM::led()
{
- /* check system state, possibly update LED to suit */
- /* re-queue ourselves to run again later */
- work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, _monitor_interval);
+ static int vehicle_status_sub_fd;
+ static int vehicle_gps_position_sub_fd;
+
+ static int num_of_cells = 0;
+ static int detected_cells_runcount = 0;
+
+ static int t_led_color[6] = { 0, 0, 0, 0, 0, 0};
+ static int t_led_blink = 0;
+ static int led_thread_runcount=0;
+ static int led_interval = 1000;
+
+ static int no_data_vehicle_status = 0;
+ static int no_data_vehicle_gps_position = 0;
+
+ static bool topic_initialized = false;
+ static bool detected_cells_blinked = false;
+ static bool led_thread_ready = true;
+
+ int num_of_used_sats = 0;
+
+ if(!topic_initialized) {
+ vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
+ orb_set_interval(vehicle_status_sub_fd, 1000);
+
+ vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
+ orb_set_interval(vehicle_gps_position_sub_fd, 1000);
+
+ topic_initialized = true;
+ }
+
+ if(led_thread_ready == true) {
+ if(!detected_cells_blinked) {
+ if(num_of_cells > 0) {
+ t_led_color[0] = LED_PURPLE;
+ }
+ if(num_of_cells > 1) {
+ t_led_color[1] = LED_PURPLE;
+ }
+ if(num_of_cells > 2) {
+ t_led_color[2] = LED_PURPLE;
+ }
+ if(num_of_cells > 3) {
+ t_led_color[3] = LED_PURPLE;
+ }
+ if(num_of_cells > 4) {
+ t_led_color[4] = LED_PURPLE;
+ }
+ t_led_color[5] = LED_OFF;
+ t_led_blink = LED_BLINK;
+ } else {
+ t_led_color[0] = led_color_1;
+ t_led_color[1] = led_color_2;
+ t_led_color[2] = led_color_3;
+ t_led_color[3] = led_color_4;
+ t_led_color[4] = led_color_5;
+ t_led_color[5] = led_color_6;
+ t_led_blink = led_blink;
+ }
+ led_thread_ready = false;
+ }
+
+ if (led_thread_runcount & 1) {
+ if (t_led_blink)
+ setLEDColor(LED_OFF);
+ led_interval = LED_OFFTIME;
+ } else {
+ setLEDColor(t_led_color[(led_thread_runcount / 2) % 6]);
+ //led_interval = (led_thread_runcount & 1) : LED_ONTIME;
+ led_interval = LED_ONTIME;
+ }
+
+ if (led_thread_runcount == 11) {
+ /* obtained data for the first file descriptor */
+ struct vehicle_status_s vehicle_status_raw;
+ struct vehicle_gps_position_s vehicle_gps_position_raw;
+
+ bool new_data_vehicle_status;
+ bool new_data_vehicle_gps_position;
+
+ orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
+
+ if (new_data_vehicle_status) {
+ orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
+ no_data_vehicle_status = 0;
+ } else {
+ no_data_vehicle_status++;
+ if(no_data_vehicle_status >= 3)
+ no_data_vehicle_status = 3;
+ }
+
+ orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
+
+ if (new_data_vehicle_gps_position) {
+ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw);
+ no_data_vehicle_gps_position = 0;
+ } else {
+ no_data_vehicle_gps_position++;
+ if(no_data_vehicle_gps_position >= 3)
+ no_data_vehicle_gps_position = 3;
+ }
+
+
+
+ /* get number of used satellites in navigation */
+ num_of_used_sats = 0;
+ for(int satloop=0; satloop<20; satloop++) {
+ if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
+ num_of_used_sats++;
+ }
+ }
+
+ if(new_data_vehicle_status || no_data_vehicle_status < 3){
+ if(num_of_cells == 0) {
+ /* looking for lipo cells that are connected */
+ printf("<blinkm> checking cells\n");
+ for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
+ if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
+ }
+ printf("<blinkm> cells found:%u\n", num_of_cells);
+
+ } else {
+ if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
+ /* LED Pattern for battery low warning */
+ led_color_1 = LED_YELLOW;
+ led_color_2 = LED_YELLOW;
+ led_color_3 = LED_YELLOW;
+ led_color_4 = LED_YELLOW;
+ led_color_5 = LED_YELLOW;
+ led_color_6 = LED_YELLOW;
+ led_blink = LED_BLINK;
+
+ } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
+ /* LED Pattern for battery critical alerting */
+ led_color_1 = LED_RED;
+ led_color_2 = LED_RED;
+ led_color_3 = LED_RED;
+ led_color_4 = LED_RED;
+ led_color_5 = LED_RED;
+ led_color_6 = LED_RED;
+ led_blink = LED_BLINK;
+
+ } else {
+ /* no battery warnings here */
+
+ if(vehicle_status_raw.flag_system_armed == false) {
+ /* system not armed */
+ led_color_1 = LED_RED;
+ led_color_2 = LED_RED;
+ led_color_3 = LED_RED;
+ led_color_4 = LED_RED;
+ led_color_5 = LED_RED;
+ led_color_6 = LED_RED;
+ led_blink = LED_NOBLINK;
+
+ } else {
+ /* armed system - initial led pattern */
+ led_color_1 = LED_RED;
+ led_color_2 = LED_RED;
+ led_color_3 = LED_RED;
+ led_color_4 = LED_OFF;
+ led_color_5 = LED_OFF;
+ led_color_6 = LED_OFF;
+ led_blink = LED_BLINK;
+
+ /* handle 4th led - flightmode indicator */
+ switch((int)vehicle_status_raw.flight_mode) {
+ case VEHICLE_FLIGHT_MODE_MANUAL:
+ led_color_4 = LED_OFF;
+ break;
+
+ case VEHICLE_FLIGHT_MODE_STAB:
+ led_color_4 = LED_YELLOW;
+ break;
+
+ case VEHICLE_FLIGHT_MODE_HOLD:
+ led_color_4 = LED_BLUE;
+ break;
+
+ case VEHICLE_FLIGHT_MODE_AUTO:
+ led_color_4 = LED_GREEN;
+ break;
+ }
+
+ if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
+ /* handling used satīs */
+ if(num_of_used_sats >= 7) {
+ led_color_1 = LED_OFF;
+ led_color_2 = LED_OFF;
+ led_color_3 = LED_OFF;
+ } else if(num_of_used_sats == 6) {
+ led_color_2 = LED_OFF;
+ led_color_3 = LED_OFF;
+ } else if(num_of_used_sats == 5) {
+ led_color_3 = LED_OFF;
+ }
+
+ } else {
+ /* no vehicle_gps_position data */
+ led_color_1 = LED_WHITE;
+ led_color_2 = LED_WHITE;
+ led_color_3 = LED_WHITE;
+
+ }
+
+ }
+ }
+ }
+ } else {
+ /* LED Pattern for general Error - no vehicle_status can retrieved */
+ led_color_1 = LED_WHITE;
+ led_color_2 = LED_WHITE;
+ led_color_3 = LED_WHITE;
+ led_color_4 = LED_WHITE;
+ led_color_5 = LED_WHITE;
+ led_color_6 = LED_WHITE;
+ led_blink = LED_BLINK;
+
+ }
+
+ /*
+ printf( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n",
+ vehicle_status_raw.voltage_battery,
+ vehicle_status_raw.flag_system_armed,
+ vehicle_status_raw.flight_mode,
+ num_of_cells,
+ no_data_vehicle_status,
+ no_data_vehicle_gps_position,
+ num_of_used_sats,
+ vehicle_gps_position_raw.fix_type,
+ vehicle_gps_position_raw.satellites_visible);
+ */
+
+ led_thread_runcount=0;
+ led_thread_ready = true;
+ led_interval = LED_OFFTIME;
+
+ if(detected_cells_runcount < 4){
+ detected_cells_runcount++;
+ } else {
+ detected_cells_blinked = true;
+ }
+
+ } else {
+ led_thread_runcount++;
+ }
+
+ if(systemstate_run == true) {
+ /* re-queue ourselves to run again later */
+ work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval);
+ } else {
+ stop_script();
+ set_rgb(0,0,0);
+ }
+}
+
+void BlinkM::setLEDColor(int ledcolor) {
+ switch (ledcolor) {
+ case LED_OFF: // off
+ set_rgb(0,0,0);
+ break;
+ case LED_RED: // red
+ set_rgb(255,0,0);
+ break;
+ case LED_YELLOW: // yellow
+ set_rgb(255,70,0);
+ break;
+ case LED_PURPLE: // purple
+ set_rgb(255,0,255);
+ break;
+ case LED_GREEN: // green
+ set_rgb(0,255,0);
+ break;
+ case LED_BLUE: // blue
+ set_rgb(0,0,255);
+ break;
+ case LED_WHITE: // white
+ set_rgb(255,255,255);
+ break;
+ }
}
int
@@ -413,7 +796,6 @@ BlinkM::get_rgb(uint8_t &r, uint8_t &g, uint8_t &b)
return ret;
}
-
int
BlinkM::get_firmware_version(uint8_t version[2])
{
@@ -443,9 +825,21 @@ blinkm_main(int argc, char *argv[])
exit(0);
}
+
if (g_blinkm == nullptr)
errx(1, "not started");
+ if (!strcmp(argv[1], "systemstate")) {
+ g_blinkm->setMode(1);
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "ledoff")) {
+ g_blinkm->setMode(0);
+ exit(0);
+ }
+
+
if (!strcmp(argv[1], "list")) {
for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++)
fprintf(stderr, " %s\n", BlinkM::script_names[i]);
@@ -458,8 +852,9 @@ blinkm_main(int argc, char *argv[])
if (fd < 0)
err(1, "can't open BlinkM device");
+ g_blinkm->setMode(0);
if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK)
exit(0);
- errx(1, "missing command, try 'start', 'list' or a script name");
-} \ No newline at end of file
+ errx(1, "missing command, try 'start', 'systemstate', 'ledoff', 'list' or a script name.");
+}
diff --git a/apps/drivers/boards/px4fmu/px4fmu_adc.c b/apps/drivers/boards/px4fmu/px4fmu_adc.c
deleted file mode 100644
index 987894163..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}; 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;
-}
diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c
index 57ffb77d3..e88d2861e 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
****************************************************************************/
@@ -150,9 +148,7 @@ __EXPORT int nsh_archinitialize(void)
int result;
/* configure the high-resolution time/callout interface */
-#ifdef CONFIG_HRT_TIMER
hrt_init();
-#endif
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
@@ -160,27 +156,21 @@ __EXPORT int nsh_archinitialize(void)
#endif
/* set up the serial DMA polling */
-#ifdef SERIAL_HAVE_DMA
- {
- static struct hrt_call serial_dma_call;
- struct timespec ts;
-
- /*
- * Poll at 1ms intervals for received bytes that have not triggered
- * a DMA event.
- */
- ts.tv_sec = 0;
- ts.tv_nsec = 1000000;
-
- hrt_call_every(&serial_dma_call,
- ts_to_abstime(&ts),
- ts_to_abstime(&ts),
- (hrt_callout)stm32_serial_dma_poll,
- NULL);
- }
-#endif
-
- message("\r\n");
+ static struct hrt_call serial_dma_call;
+ struct timespec ts;
+
+ /*
+ * Poll at 1ms intervals for received bytes that have not triggered
+ * a DMA event.
+ */
+ ts.tv_sec = 0;
+ ts.tv_nsec = 1000000;
+
+ hrt_call_every(&serial_dma_call,
+ ts_to_abstime(&ts),
+ ts_to_abstime(&ts),
+ (hrt_callout)stm32_serial_dma_poll,
+ NULL);
// initial LED state
drv_led_start();
@@ -209,8 +199,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 +222,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);
+ 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 a0342ac8a..f77d237a7 100644
--- a/apps/drivers/boards/px4io/px4io_internal.h
+++ b/apps/drivers/boards/px4io/px4io_internal.h
@@ -61,28 +61,6 @@
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
-/* R/C in/out channels **************************************************************/
-
-/* XXX just GPIOs for now - eventually timer pins */
-
-#define GPIO_CH1_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN0)
-#define GPIO_CH2_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN1)
-#define GPIO_CH3_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN8)
-#define GPIO_CH4_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN9)
-#define GPIO_CH5_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN6)
-#define GPIO_CH6_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN7)
-#define GPIO_CH7_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN0)
-#define GPIO_CH8_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN1)
-
-#define GPIO_CH1_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
-#define GPIO_CH2_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
-#define GPIO_CH3_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
-#define GPIO_CH4_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
-#define GPIO_CH5_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
-#define GPIO_CH6_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
-#define GPIO_CH7_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
-#define GPIO_CH8_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
-
/* Safety switch button *************************************************************/
#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
@@ -98,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/drv_mixer.h b/apps/drivers/drv_mixer.h
index 793e86b32..9f43015d9 100644
--- a/apps/drivers/drv_mixer.h
+++ b/apps/drivers/drv_mixer.h
@@ -100,28 +100,13 @@ struct mixer_simple_s {
*/
#define MIXERIOCADDSIMPLE _MIXERIOC(2)
-/** multirotor output definition */
-struct mixer_rotor_output_s {
- float angle; /**< rotor angle clockwise from forward in radians */
- float distance; /**< motor distance from centre in arbitrary units */
-};
-
-/** multirotor mixer */
-struct mixer_multirotor_s {
- uint8_t rotor_count;
- struct mixer_control_s controls[4]; /**< controls are roll, pitch, yaw, thrust */
- struct mixer_rotor_output_s rotors[0]; /**< actual size of the array is set by rotor_count */
-};
-
-/**
- * Add a multirotor mixer in (struct mixer_multirotor_s *)arg
- */
-#define MIXERIOCADDMULTIROTOR _MIXERIOC(3)
+/* _MIXERIOC(3) was deprecated */
+/* _MIXERIOC(4) was deprecated */
/**
- * Add mixers(s) from a the file in (const char *)arg
+ * Add mixer(s) from the buffer in (const char *)arg
*/
-#define MIXERIOCLOADFILE _MIXERIOC(4)
+#define MIXERIOCLOADBUF _MIXERIOC(5)
/*
* XXX Thoughts for additional operations:
diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h
index b2fee65ac..c110cd5cb 100644
--- a/apps/drivers/drv_pwm_output.h
+++ b/apps/drivers/drv_pwm_output.h
@@ -103,6 +103,9 @@ ORB_DECLARE(output_pwm);
/** disarm all servo outputs (stop generating pulses) */
#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1)
+/** set update rate in Hz */
+#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
+
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp
index 67b16aa42..fe9b281f6 100644
--- a/apps/drivers/hil/hil.cpp
+++ b/apps/drivers/hil/hil.cpp
@@ -68,11 +68,11 @@
#include <drivers/device/device.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
-// #include <drivers/boards/HIL/HIL_internal.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_mixer.h>
#include <systemlib/systemlib.h>
#include <systemlib/mixer/mixer.h>
-#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
@@ -382,7 +382,6 @@ HIL::task_main()
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
- usleep(1000000);
continue;
}
@@ -396,16 +395,27 @@ HIL::task_main()
if (_mixers != nullptr) {
/* do mixing */
- _mixers->mix(&outputs.output[0], num_outputs);
+ outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+ outputs.timestamp = hrt_absolute_time();
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
- /* scale for PWM output 900 - 2100us */
- outputs.output[i] = 1500 + (600 * outputs.output[i]);
-
- /* output to the servo */
- // up_pwm_servo_set(i, outputs.output[i]);
+ /* last resort: catch NaN, INF and out-of-band errors */
+ if (i < (unsigned)outputs.noutputs &&
+ isfinite(outputs.output[i]) &&
+ outputs.output[i] >= -1.0f &&
+ outputs.output[i] <= 1.0f) {
+ /* scale for PWM output 900 - 2100us */
+ outputs.output[i] = 1500 + (600 * outputs.output[i]);
+ } else {
+ /*
+ * Value is NaN, INF or out of band - set to the minimum value.
+ * This will be clearly visible on the servo status and will limit the risk of accidentally
+ * spinning motors. It would be deadly in flight.
+ */
+ outputs.output[i] = 900;
+ }
}
/* and publish for anyone that cares to see */
@@ -419,9 +429,6 @@ HIL::task_main()
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
-
- /* update PWM servo armed status */
- // up_pwm_servo_arm(aa.armed);
}
}
@@ -503,6 +510,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
// up_pwm_servo_arm(false);
break;
+ case PWM_SERVO_SET_UPDATE_RATE:
+ g_hil->set_pwm_rate(arg);
+ break;
+
case PWM_SERVO_SET(2):
case PWM_SERVO_SET(3):
if (_mode != MODE_4PWM) {
@@ -577,26 +588,19 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
- case MIXERIOCADDMULTIROTOR:
- /* XXX not yet supported */
- ret = -ENOTTY;
- break;
-
- case MIXERIOCLOADFILE: {
- const char *path = (const char *)arg;
+ case MIXERIOCLOADBUF: {
+ const char *buf = (const char *)arg;
+ unsigned buflen = strnlen(buf, 1024);
- if (_mixers != nullptr) {
- delete _mixers;
- _mixers = nullptr;
- }
+ if (_mixers == nullptr)
+ _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
- _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers == nullptr) {
ret = -ENOMEM;
+
} else {
- debug("loading mixers from %s", path);
- ret = _mixers->load_from_file(path);
+ ret = _mixers->load_from_buf(buf, buflen);
if (ret != 0) {
debug("mixer load failed with %d", ret);
@@ -605,10 +609,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
}
}
-
break;
}
+
default:
ret = -ENOTTY;
break;
diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp
index a995f6214..430d18c6d 100644
--- a/apps/drivers/px4fmu/fmu.cpp
+++ b/apps/drivers/px4fmu/fmu.cpp
@@ -58,6 +58,7 @@
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
#include <drivers/boards/px4fmu/px4fmu_internal.h>
+#include <drivers/drv_hrt.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
@@ -65,6 +66,7 @@
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <systemlib/err.h>
@@ -97,6 +99,7 @@ private:
int _t_actuators;
int _t_armed;
orb_advert_t _t_outputs;
+ orb_advert_t _t_actuators_effective;
unsigned _num_outputs;
bool _primary_pwm_device;
@@ -162,6 +165,7 @@ PX4FMU::PX4FMU() :
_t_actuators(-1),
_t_armed(-1),
_t_outputs(0),
+ _t_actuators_effective(0),
_num_outputs(0),
_primary_pwm_device(false),
_task_should_exit(false),
@@ -319,6 +323,13 @@ PX4FMU::task_main()
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
+ /* advertise the effective control inputs */
+ actuator_controls_effective_s controls_effective;
+ memset(&controls_effective, 0, sizeof(controls_effective));
+ /* advertise the effective control inputs */
+ _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
+ &controls_effective);
+
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
@@ -336,8 +347,16 @@ PX4FMU::task_main()
if (_current_update_rate != _update_rate) {
int update_rate_in_ms = int(1000 / _update_rate);
- if (update_rate_in_ms < 2)
+ /* reject faster than 500 Hz updates */
+ if (update_rate_in_ms < 2) {
update_rate_in_ms = 2;
+ _update_rate = 500;
+ }
+ /* reject slower than 50 Hz updates */
+ if (update_rate_in_ms > 20) {
+ update_rate_in_ms = 20;
+ _update_rate = 50;
+ }
orb_set_interval(_t_actuators, update_rate_in_ms);
up_pwm_servo_set_rate(_update_rate);
@@ -364,20 +383,39 @@ PX4FMU::task_main()
if (_mixers != nullptr) {
/* do mixing */
- _mixers->mix(&outputs.output[0], num_outputs);
+ outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+ outputs.timestamp = hrt_absolute_time();
+
+ // XXX output actual limited values
+ memcpy(&controls_effective, &_controls, sizeof(controls_effective));
+
+ orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
- /* scale for PWM output 900 - 2100us */
- outputs.output[i] = 1500 + (600 * outputs.output[i]);
+ /* last resort: catch NaN, INF and out-of-band errors */
+ if (i < outputs.noutputs &&
+ isfinite(outputs.output[i]) &&
+ outputs.output[i] >= -1.0f &&
+ outputs.output[i] <= 1.0f) {
+ /* scale for PWM output 900 - 2100us */
+ outputs.output[i] = 1500 + (600 * outputs.output[i]);
+ } else {
+ /*
+ * Value is NaN, INF or out of band - set to the minimum value.
+ * This will be clearly visible on the servo status and will limit the risk of accidentally
+ * spinning motors. It would be deadly in flight.
+ */
+ outputs.output[i] = 900;
+ }
/* output to the servo */
up_pwm_servo_set(i, outputs.output[i]);
}
/* and publish for anyone that cares to see */
- orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
+ orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
}
}
@@ -394,6 +432,7 @@ PX4FMU::task_main()
}
::close(_t_actuators);
+ ::close(_t_actuators_effective);
::close(_t_armed);
/* make sure servos are off */
@@ -470,6 +509,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
up_pwm_servo_arm(false);
break;
+ case PWM_SERVO_SET_UPDATE_RATE:
+ set_pwm_rate(arg);
+ break;
+
case PWM_SERVO_SET(2):
case PWM_SERVO_SET(3):
if (_mode != MODE_4PWM) {
@@ -500,7 +543,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
/* FALLTHROUGH */
case PWM_SERVO_GET(0):
case PWM_SERVO_GET(1): {
- channel = cmd - PWM_SERVO_SET(0);
+ channel = cmd - PWM_SERVO_GET(0);
*(servo_position_t *)arg = up_pwm_servo_get(channel);
break;
}
@@ -544,28 +587,19 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
- case MIXERIOCADDMULTIROTOR:
- /* XXX not yet supported */
- ret = -ENOTTY;
- break;
+ case MIXERIOCLOADBUF: {
+ const char *buf = (const char *)arg;
+ unsigned buflen = strnlen(buf, 1024);
- case MIXERIOCLOADFILE: {
- const char *path = (const char *)arg;
-
- if (_mixers != nullptr) {
- delete _mixers;
- _mixers = nullptr;
- }
-
- _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
+ if (_mixers == nullptr)
+ _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers == nullptr) {
ret = -ENOMEM;
} else {
- debug("loading mixers from %s", path);
- ret = _mixers->load_from_file(path);
+ ret = _mixers->load_from_buf(buf, buflen);
if (ret != 0) {
debug("mixer load failed with %d", ret);
@@ -574,7 +608,6 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
}
}
-
break;
}
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 9f3dba047..2c2c236ca 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -55,12 +55,14 @@
#include <unistd.h>
#include <termios.h>
#include <fcntl.h>
+#include <math.h>
#include <arch/board/board.h>
#include <drivers/device/device.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
@@ -69,10 +71,15 @@
#include <systemlib/hx_stream.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
+#include <systemlib/scheduling_priorities.h>
+#include <systemlib/param/param.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_effective.h>
#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"
@@ -88,10 +95,18 @@ public:
virtual int ioctl(file *filp, int cmd, unsigned long arg);
+ /**
+ * Set the PWM via serial update rate
+ * @warning this directly affects CPU load
+ */
+ int set_pwm_rate(int hz);
+
bool dump_one;
private:
- static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS;
+ // XXX
+ static const unsigned _max_actuators = PX4IO_CONTROL_CHANNELS;
+ unsigned _update_rate; ///< serial send rate in Hz
int _serial_fd; ///< serial interface to PX4IO
hx_stream_t _io_stream; ///< HX protocol stream
@@ -103,19 +118,30 @@ private:
int _t_actuators; ///< actuator output topic
actuator_controls_s _controls; ///< actuator outputs
+ orb_advert_t _t_actuators_effective; ///< effective actuator controls topic
+ actuator_controls_effective_s _controls_effective; ///< effective controls
+
int _t_armed; ///< system armed control topic
actuator_armed_s _armed; ///< system armed state
+ int _t_vstatus; ///< system / vehicle status
+ vehicle_status_s _vstatus; ///< overall system state
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
- MixerGroup *_mixers; ///< loaded mixers
+ const char *volatile _mix_buf; ///< mixer text buffer
+ volatile unsigned _mix_buf_len; ///< size of the mixer text buffer
bool _primary_pwm_device; ///< true if we are the default PWM output
+ uint32_t _relays; ///< state of the PX4IO relays, one bit per relay
+
volatile bool _switch_armed; ///< PX4IO switch armed state
// XXX how should this work?
@@ -158,6 +184,11 @@ private:
void config_send();
/**
+ * Send a buffer containing mixer text to PX4IO
+ */
+ int mixer_send(const char *buf, unsigned buflen);
+
+ /**
* Mixer control callback; invoked to fetch a control from a specific
* group/index during mixing.
*/
@@ -178,19 +209,24 @@ PX4IO *g_dev;
PX4IO::PX4IO() :
CDev("px4io", "/dev/px4io"),
dump_one(false),
+ _update_rate(50),
_serial_fd(-1),
_io_stream(nullptr),
_task(-1),
_task_should_exit(false),
_connected(false),
_t_actuators(-1),
+ _t_actuators_effective(-1),
_t_armed(-1),
+ _t_vstatus(-1),
_t_outputs(-1),
- _mixers(nullptr),
+ _mix_buf(nullptr),
+ _mix_buf_len(0),
_primary_pwm_device(false),
+ _relays(0),
_switch_armed(false),
_send_needed(false),
- _config_needed(false)
+ _config_needed(true)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@@ -208,6 +244,7 @@ PX4IO::~PX4IO()
/* give it another 100ms */
usleep(100000);
}
+
/* well, kill it anyway, though this will probably crash */
if (_task != -1)
task_delete(_task);
@@ -237,7 +274,8 @@ PX4IO::init()
}
/* start the IO interface task */
- _task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
+ _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
+
if (_task < 0) {
debug("task start failed: %d", errno);
return -errno;
@@ -249,16 +287,30 @@ PX4IO::init()
debug("PX4IO connected");
break;
}
+
usleep(100000);
}
+
if (!_connected) {
/* error here will result in everything being torn down */
log("PX4IO not responding");
return -EIO;
}
+
return OK;
}
+int
+PX4IO::set_pwm_rate(int hz)
+{
+ if (hz > 0 && hz <= 400) {
+ _update_rate = hz;
+ return OK;
+ } else {
+ return -EINVAL;
+ }
+}
+
void
PX4IO::task_main_trampoline(int argc, char *argv[])
{
@@ -269,6 +321,7 @@ void
PX4IO::task_main()
{
log("starting");
+ unsigned update_rate_in_ms;
/* open the serial port */
_serial_fd = ::open("/dev/ttyS2", O_RDWR);
@@ -290,10 +343,12 @@ PX4IO::task_main()
/* protocol stream */
_io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
+
if (_io_stream == nullptr) {
log("failed to allocate HX protocol stream");
goto out;
}
+
hx_stream_set_counters(_io_stream,
perf_alloc(PC_COUNT, "PX4IO frames transmitted"),
perf_alloc(PC_COUNT, "PX4IO frames received"),
@@ -306,12 +361,20 @@ PX4IO::task_main()
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
/* convert the update rate in hz to milliseconds, rounding down if necessary */
- //int update_rate_in_ms = int(1000 / _update_rate);
- orb_set_interval(_t_actuators, 20); /* XXX 50Hz hardcoded for now */
+ update_rate_in_ms = 1000 / _update_rate;
+ orb_set_interval(_t_actuators, update_rate_in_ms);
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
+ _t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
+ orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */
+
+ /* advertise the limited control inputs */
+ memset(&_controls_effective, 0, sizeof(_controls_effective));
+ _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1),
+ &_controls_effective);
+
/* advertise the mixed control outputs */
memset(&_outputs, 0, sizeof(_outputs));
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
@@ -321,22 +384,33 @@ 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];
+ pollfd fds[4];
fds[0].fd = _serial_fd;
fds[0].events = POLLIN;
fds[1].fd = _t_actuators;
fds[1].events = POLLIN;
fds[2].fd = _t_armed;
fds[2].events = POLLIN;
+ fds[3].fd = _t_vstatus;
+ fds[3].events = POLLIN;
- log("ready");
+ debug("ready");
+
+ /* lock against the ioctl handler */
+ lock();
/* loop handling received serial bytes */
while (!_task_should_exit) {
/* sleep waiting for data, but no more than 100ms */
+ unlock();
int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100);
+ lock();
/* this would be bad... */
if (ret < 0) {
@@ -353,37 +427,32 @@ PX4IO::task_main()
if (fds[0].revents & POLLIN)
io_recv();
- /* if we have new data from the ORB, go handle it */
+ /* if we have new control data from the ORB, handle it */
if (fds[1].revents & POLLIN) {
/* get controls */
- orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
+ orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+ ORB_ID(actuator_controls_1), _t_actuators, &_controls);
- /* mix */
- if (_mixers != nullptr) {
- /* XXX is this the right count? */
- _mixers->mix(&_outputs.output[0], _max_actuators);
+ /* scale controls to PWM (temporary measure) */
+ for (unsigned i = 0; i < _max_actuators; i++)
+ _outputs.output[i] = 1500 + (600 * _controls.control[i]);
- /* convert to PWM values */
- for (unsigned i = 0; i < _max_actuators; i++)
- _outputs.output[i] = 1500 + (600 * _outputs.output[i]);
-
- /* and flag for update */
- _send_needed = true;
- }
+ /* and flag for update */
+ _send_needed = true;
}
+ /* if we have an arming state update, handle it */
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed);
_send_needed = true;
}
- }
- /* send an update to IO if required */
- if (_send_needed) {
- _send_needed = false;
- io_send();
+ if (fds[3].revents & POLLIN) {
+ orb_copy(ORB_ID(vehicle_status), _t_vstatus, &_vstatus);
+ _send_needed = true;
+ }
}
/* send a config packet to IO if required */
@@ -391,14 +460,32 @@ PX4IO::task_main()
_config_needed = false;
config_send();
}
+
+ /* send a mixer update if needed */
+ if (_mix_buf != nullptr) {
+ mixer_send(_mix_buf, _mix_buf_len);
+
+ /* clear the buffer record so the ioctl handler knows we're done */
+ _mix_buf = nullptr;
+ _mix_buf_len = 0;
+ }
+
+ /* send an update to IO if required */
+ if (_send_needed) {
+ _send_needed = false;
+ io_send();
+ }
}
+ unlock();
+
out:
debug("exiting");
/* kill the HX stream */
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
+
::close(_serial_fd);
/* clean up the alternate device node */
@@ -451,25 +538,27 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
{
const px4io_report *rep = (const px4io_report *)buffer;
- lock();
+// lock();
/* sanity-check the received frame size */
if (bytes_received != sizeof(px4io_report)) {
debug("got %u expected %u", bytes_received, sizeof(px4io_report));
goto out;
}
+
if (rep->i2f_magic != I2F_MAGIC) {
debug("bad magic");
goto out;
}
+
_connected = true;
/* publish raw rc channel values from IO if valid channels are present */
if (rep->channel_count > 0) {
_input_rc.timestamp = hrt_absolute_time();
_input_rc.channel_count = rep->channel_count;
- for (int i = 0; i < rep->channel_count; i++)
- {
+
+ for (int i = 0; i < rep->channel_count; i++) {
_input_rc.values[i] = rep->rc_channel[i];
}
@@ -479,6 +568,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 */
@@ -486,13 +592,16 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
dump_one = false;
printf("IO: %s armed ", rep->armed ? "" : "not");
+
for (unsigned i = 0; i < rep->channel_count; i++)
printf("%d: %d ", i, rep->rc_channel[i]);
+
printf("\n");
}
out:
- unlock();
+// unlock();
+ return;
}
void
@@ -504,18 +613,29 @@ PX4IO::io_send()
cmd.f2i_magic = F2I_MAGIC;
/* set outputs */
- for (unsigned i = 0; i < _max_actuators; i++)
- cmd.servo_command[i] = _outputs.output[i];
-
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ cmd.output_control[i] = _outputs.output[i];
+ }
/* publish as we send */
- orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs);
+ _outputs.timestamp = hrt_absolute_time();
+ /* XXX needs to be based off post-mix values from the IO side */
+ orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs);
- // XXX relays
+ /* update relays */
+ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
+ cmd.relay_state[i] = (_relays & (1<< i)) ? true : false;
- /* armed and not locked down */
+ /* armed and not locked down -> arming ok */
cmd.arm_ok = (_armed.armed && !_armed.lockdown);
-
+ /* indicate that full autonomous position control / vector flight mode is available */
+ cmd.vector_flight_mode_ok = _vstatus.flag_vector_flight_mode_ok;
+ /* allow manual override on IO (not allowed for multirotors or other systems with SAS) */
+ cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok;
+ /* set desired PWM output rate */
+ cmd.servo_rate = _update_rate;
+
ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
+
if (ret)
debug("send error %d", ret);
}
@@ -528,12 +648,89 @@ PX4IO::config_send()
cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
+ int val;
+
+ /* maintaing the standard order of Roll, Pitch, Yaw, Throttle */
+ param_get(param_find("RC_MAP_ROLL"), &val);
+ cfg.rc_map[0] = val;
+ param_get(param_find("RC_MAP_PITCH"), &val);
+ cfg.rc_map[1] = val;
+ param_get(param_find("RC_MAP_YAW"), &val);
+ cfg.rc_map[2] = val;
+ param_get(param_find("RC_MAP_THROTTLE"), &val);
+ cfg.rc_map[3] = val;
+
+ /* set the individual channel properties */
+ char nbuf[16];
+ float float_val;
+ for (unsigned i = 0; i < 4; i++) {
+ sprintf(nbuf, "RC%d_MIN", i + 1);
+ param_get(param_find(nbuf), &float_val);
+ cfg.rc_min[i] = float_val;
+ }
+ for (unsigned i = 0; i < 4; i++) {
+ sprintf(nbuf, "RC%d_TRIM", i + 1);
+ param_get(param_find(nbuf), &float_val);
+ cfg.rc_trim[i] = float_val;
+ }
+ for (unsigned i = 0; i < 4; i++) {
+ sprintf(nbuf, "RC%d_MAX", i + 1);
+ param_get(param_find(nbuf), &float_val);
+ cfg.rc_max[i] = float_val;
+ }
+ for (unsigned i = 0; i < 4; i++) {
+ sprintf(nbuf, "RC%d_REV", i + 1);
+ param_get(param_find(nbuf), &float_val);
+ cfg.rc_rev[i] = float_val;
+ }
+ for (unsigned i = 0; i < 4; i++) {
+ sprintf(nbuf, "RC%d_DZ", i + 1);
+ param_get(param_find(nbuf), &float_val);
+ cfg.rc_dz[i] = float_val;
+ }
+
ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
+
if (ret)
debug("config error %d", ret);
}
int
+PX4IO::mixer_send(const char *buf, unsigned buflen)
+{
+ uint8_t frame[HX_STREAM_MAX_FRAME];
+ px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
+
+ msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
+ msg->action = F2I_MIXER_ACTION_RESET;
+
+ do {
+ unsigned count = buflen;
+
+ if (count > F2I_MIXER_MAX_TEXT)
+ count = F2I_MIXER_MAX_TEXT;
+
+ if (count > 0) {
+ memcpy(&msg->text[0], buf, count);
+ buf += count;
+ buflen -= count;
+ }
+
+ int ret = hx_stream_send(_io_stream, msg, sizeof(px4io_mixdata) + count);
+
+ if (ret) {
+ log("mixer send error %d", ret);
+ return ret;
+ }
+
+ msg->action = F2I_MIXER_ACTION_APPEND;
+
+ } while (buflen > 0);
+
+ return 0;
+}
+
+int
PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
{
int ret = OK;
@@ -554,9 +751,14 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
_send_needed = true;
break;
+ case PWM_SERVO_SET_UPDATE_RATE:
+ // not supported yet
+ ret = -EINVAL;
+ break;
+
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
- /* fake an update to the selected servo channel */
+ /* fake an update to the selected 'servo' channel */
if ((arg >= 900) && (arg <= 2100)) {
_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
_send_needed = true;
@@ -572,68 +774,53 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
*(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)];
break;
- case MIXERIOCGETOUTPUTCOUNT:
- *(unsigned *)arg = _max_actuators;
+ case GPIO_RESET:
+ _relays = 0;
+ _send_needed = true;
break;
- case MIXERIOCRESET:
- if (_mixers != nullptr) {
- delete _mixers;
- _mixers = nullptr;
+ case GPIO_SET:
+ case GPIO_CLEAR:
+ /* make sure only valid bits are being set */
+ if ((arg & ((1UL << PX4IO_RELAY_CHANNELS) - 1)) != arg) {
+ ret = EINVAL;
+ break;
}
-
+ if (cmd == GPIO_SET) {
+ _relays |= arg;
+ } else {
+ _relays &= ~arg;
+ }
+ _send_needed = true;
break;
- case MIXERIOCADDSIMPLE: {
- mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
-
- /* build the new mixer from the supplied argument */
- SimpleMixer *mixer = new SimpleMixer(control_callback,
- (uintptr_t)&_controls, mixinfo);
-
- /* validate the new mixer */
- if (mixer->check()) {
- delete mixer;
- ret = -EINVAL;
-
- } else {
- /* if we don't have a group yet, allocate one */
- if (_mixers == nullptr)
- _mixers = new MixerGroup(control_callback,
- (uintptr_t)&_controls);
-
- /* add the new mixer to the group */
- _mixers->add_mixer(mixer);
- }
+ case GPIO_GET:
+ *(uint32_t *)arg = _relays;
+ break;
- }
+ case MIXERIOCGETOUTPUTCOUNT:
+ *(unsigned *)arg = PX4IO_CONTROL_CHANNELS;
break;
- case MIXERIOCADDMULTIROTOR:
- /* XXX not yet supported */
- ret = -ENOTTY;
+ case MIXERIOCRESET:
+ ret = 0; /* load always resets */
break;
- case MIXERIOCLOADFILE: {
- MixerGroup *newmixers;
- const char *path = (const char *)arg;
+ case MIXERIOCLOADBUF:
- /* allocate a new mixer group and load it from the file */
- newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
+ /* set the buffer up for transfer */
+ _mix_buf = (const char *)arg;
+ _mix_buf_len = strnlen(_mix_buf, 1024);
- if (newmixers->load_from_file(path) != 0) {
- delete newmixers;
- ret = -EINVAL;
- }
+ /* drop the lock and wait for the thread to clear the transmit */
+ unlock();
- /* swap the new mixers in for the old */
- if (_mixers != nullptr) {
- delete _mixers;
- }
+ while (_mix_buf != nullptr)
+ usleep(1000);
- _mixers = newmixers;
+ lock();
- }
+ ret = 0;
break;
default:
@@ -675,13 +862,20 @@ test(void)
close(fd);
+ actuator_armed_s aa;
+
+ aa.armed = true;
+ aa.lockdown = false;
+
+ orb_advertise(ORB_ID(actuator_armed), &aa);
+
exit(0);
}
void
monitor(void)
{
- unsigned cancels = 4;
+ unsigned cancels = 3;
printf("Hit <enter> three times to exit monitor mode\n");
for (;;) {
@@ -694,6 +888,7 @@ monitor(void)
if (fds[0].revents == POLLIN) {
int c;
read(0, &c, 1);
+
if (cancels-- == 0)
exit(0);
}
@@ -702,6 +897,7 @@ monitor(void)
g_dev->dump_one = true;
}
}
+
}
int
@@ -723,12 +919,51 @@ px4io_main(int argc, char *argv[])
errx(1, "driver init failed");
}
+ /* look for the optional pwm update rate for the supported modes */
+ if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) {
+ if (argc > 2 + 1) {
+ g_dev->set_pwm_rate(atoi(argv[2 + 1]));
+ } else {
+ fprintf(stderr, "missing argument for pwm update rate (-u)\n");
+ return 1;
+ }
+ }
+
exit(0);
}
+ if (!strcmp(argv[1], "stop")) {
+
+ if (g_dev != nullptr) {
+ /* stop the driver */
+ delete g_dev;
+ } else {
+ errx(1, "not loaded");
+ }
+ exit(0);
+ }
+
+
+ if (!strcmp(argv[1], "status")) {
+
+ if (g_dev != nullptr)
+ printf("[px4io] loaded\n");
+ else
+ printf("[px4io] not loaded\n");
+
+ exit(0);
+ }
+
/* note, stop not currently implemented */
if (!strcmp(argv[1], "update")) {
+
+ if (g_dev != nullptr) {
+ printf("[px4io] loaded, detaching first\n");
+ /* stop the driver */
+ delete g_dev;
+ }
+
PX4IO_Uploader *up;
const char *fn[3];
@@ -780,8 +1015,9 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "test"))
test();
+
if (!strcmp(argv[1], "monitor"))
monitor();
- errx(1, "need a command, try 'start', 'test', 'monitor' or 'update'");
+ errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor' or 'update'");
}
diff --git a/apps/drivers/px4io/uploader.cpp b/apps/drivers/px4io/uploader.cpp
index 8b354ff60..2de33f410 100644
--- a/apps/drivers/px4io/uploader.cpp
+++ b/apps/drivers/px4io/uploader.cpp
@@ -51,6 +51,50 @@
#include "uploader.h"
+static const uint32_t crctab[] =
+{
+ 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
+ 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
+ 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+ 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
+ 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
+ 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+ 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
+ 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
+ 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+ 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
+ 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
+ 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+ 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
+ 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
+ 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+ 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
+ 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
+ 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+ 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
+ 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
+ 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+ 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
+ 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
+ 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+ 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
+ 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
+ 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+ 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
+ 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
+ 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+ 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
+ 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
+};
+
+static uint32_t
+crc32(const uint8_t *src, unsigned len, unsigned state)
+{
+ for (unsigned i = 0; i < len; i++)
+ state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8);
+ return state;
+}
+
PX4IO_Uploader::PX4IO_Uploader() :
_io_fd(-1),
_fw_fd(-1)
@@ -110,6 +154,17 @@ PX4IO_Uploader::upload(const char *filenames[])
}
}
+ ret = get_info(INFO_BL_REV, bl_rev);
+
+ if (ret == OK) {
+ if (bl_rev <= BL_REV) {
+ log("found bootloader revision: %d", bl_rev);
+ } else {
+ log("found unsupported bootloader revision %d, exiting", bl_rev);
+ return OK;
+ }
+ }
+
ret = erase();
if (ret != OK) {
@@ -124,7 +179,11 @@ PX4IO_Uploader::upload(const char *filenames[])
continue;
}
- ret = verify();
+ if (bl_rev <= 2)
+ ret = verify_rev2();
+ else if(bl_rev == 3) {
+ ret = verify_rev3();
+ }
if (ret != OK) {
log("verify failed");
@@ -190,6 +249,7 @@ PX4IO_Uploader::drain()
do {
ret = recv(c, 250);
+
if (ret == OK) {
//log("discard 0x%02x", c);
}
@@ -319,7 +379,7 @@ PX4IO_Uploader::program()
}
int
-PX4IO_Uploader::verify()
+PX4IO_Uploader::verify_rev2()
{
uint8_t file_buf[PROG_MULTI_MAX];
ssize_t count;
@@ -380,6 +440,74 @@ PX4IO_Uploader::verify()
}
int
+PX4IO_Uploader::verify_rev3()
+{
+ int ret;
+ uint8_t file_buf[4];
+ ssize_t count;
+ uint32_t sum = 0;
+ uint32_t bytes_read = 0;
+ uint32_t fw_size = 0;
+ uint32_t crc = 0;
+ uint8_t fill_blank = 0xff;
+
+ log("verify...");
+ lseek(_fw_fd, 0, SEEK_SET);
+
+ ret = get_info(INFO_FLASH_SIZE, fw_size);
+ send(PROTO_EOC);
+
+ if (ret != OK) {
+ log("could not read firmware size");
+ return ret;
+ }
+
+ /* read through the firmware file again and calculate the checksum*/
+ while (true) {
+ lseek(_fw_fd, 0, SEEK_CUR);
+ count = read(_fw_fd, file_buf, sizeof(file_buf));
+
+ /* set the rest to ff */
+ if (count == 0) {
+ break;
+ }
+ /* stop if the file cannot be read */
+ if (count < 0)
+ return -errno;
+
+ /* calculate crc32 sum */
+ sum = crc32((uint8_t *)&file_buf, sizeof(file_buf), sum);
+
+ bytes_read += count;
+ }
+
+ /* fill the rest with 0xff */
+ while (bytes_read < fw_size) {
+ sum = crc32(&fill_blank, sizeof(fill_blank), sum);
+ bytes_read += sizeof(fill_blank);
+ }
+
+ /* request CRC from IO */
+ send(PROTO_GET_CRC);
+ send(PROTO_EOC);
+
+ ret = recv((uint8_t*)(&crc), sizeof(crc));
+
+ if (ret != OK) {
+ log("did not receive CRC checksum");
+ return ret;
+ }
+
+ /* compare the CRC sum from the IO with the one calculated */
+ if (sum != crc) {
+ log("CRC wrong: received: %d, expected: %d", crc, sum);
+ return -EINVAL;
+ }
+
+ return OK;
+}
+
+int
PX4IO_Uploader::reboot()
{
send(PROTO_REBOOT);
diff --git a/apps/drivers/px4io/uploader.h b/apps/drivers/px4io/uploader.h
index 8d41992f8..b8a3a2794 100644
--- a/apps/drivers/px4io/uploader.h
+++ b/apps/drivers/px4io/uploader.h
@@ -64,21 +64,25 @@ private:
PROTO_CHIP_VERIFY = 0x24,
PROTO_PROG_MULTI = 0x27,
PROTO_READ_MULTI = 0x28,
+ PROTO_GET_CRC = 0x29,
PROTO_REBOOT = 0x30,
INFO_BL_REV = 1, /**< bootloader protocol revision */
- BL_REV = 2, /**< supported bootloader protocol */
+ BL_REV = 3, /**< supported bootloader protocol */
INFO_BOARD_ID = 2, /**< board type */
INFO_BOARD_REV = 3, /**< board revision */
INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */
PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */
READ_MULTI_MAX = 60, /**< protocol max is 255, something overflows with >= 64 */
+
};
int _io_fd;
int _fw_fd;
+ uint32_t bl_rev; /**< bootloader revision */
+
void log(const char *fmt, ...);
int recv(uint8_t &c, unsigned timeout = 1000);
@@ -91,7 +95,8 @@ private:
int get_info(int param, uint32_t &val);
int erase();
int program();
- int verify();
+ int verify_rev2();
+ int verify_rev3();
int reboot();
int compare(bool &identical);
};
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..911def943
--- /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 default channel set for PX4FMU - should be configurable */
+ g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
+
+ 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);
+}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-17 22:02:56 +0000