dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #148 from PX4/adc_integration 

Selected adjustments / fixes to make old apps compatible with new-style ADC
This commit is contained in:
px4dev 2013-01-13 13:43:45 -08:00
parent 58ea9fbf01 9ac2684ca0
commit ff146cc5b1
9 changed files with 220 additions and 262 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
apps/commander/commander.c
View File

@ -1319,8 +1319,8 @@ int commander_thread_main(int argc, char *argv[])
uint16_t counter = 0;
uint8_t flight_env;
/* Initialize to 3.0V to make sure the low-pass loads below valid threshold */
float battery_voltage = 12.0f;
/* Initialize to 0.0V */
float battery_voltage = 0.0f;
bool battery_voltage_valid = true;
bool low_battery_voltage_actions_done = false;
bool critical_battery_voltage_actions_done = false;
@ -1473,8 +1473,8 @@ int commander_thread_main(int argc, char *argv[])
last_local_position_time = local_position.timestamp;
}
/* update battery status */
orb_check(battery_sub, &new_data);
if (new_data) {
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
battery_voltage = battery.voltage_v;

2
apps/drivers/boards/px4fmu/px4fmu_init.c
View File

@ -226,7 +226,7 @@ __EXPORT int nsh_archinitialize(void)
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
stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
return OK;
}

4
apps/drivers/stm32/adc/adc.cpp
View File

@ -366,8 +366,8 @@ 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));
/* 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");

94
apps/px4/tests/test_adc.c
View File

@ -53,96 +53,40 @@
#include "tests.h"
#include <nuttx/analog/adc.h>
#include <drivers/drv_adc.h>
#include <systemlib/err.h>
int test_adc(int argc, char *argv[])
{
int fd0 = 0;
int ret = 0;
int fd = open(ADC_DEVICE_PATH, O_RDONLY);
#pragma pack(push,1)
struct adc_msg4_s {
uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */
int32_t am_data1; /**< ADC convert result 1 (4 bytes) */
uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */
int32_t am_data2; /**< ADC convert result 2 (4 bytes) */
uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */
int32_t am_data3; /**< ADC convert result 3 (4 bytes) */
uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */
int32_t am_data4; /**< ADC convert result 4 (4 bytes) */
};
#pragma pack(pop)
if (fd < 0)
err(1, "can't open ADC device");
struct adc_msg4_s sample1;
for (unsigned i = 0; i < 5; i++) {
/* make space for a maximum of eight channels */
struct adc_msg_s data[8];
/* read all channels available */
ssize_t count = read(fd, data, sizeof(data));
ssize_t nbytes;
int j;
int errval;
if (count < 0)
goto errout_with_dev;
fd0 = open("/dev/adc0", O_RDONLY | O_NONBLOCK);
unsigned channels = count / sizeof(data[0]);
if (fd0 <= 0) {
message("/dev/adc0 open fail: %d\n", errno);
return ERROR;
} else {
message("opened /dev/adc0 successfully\n");
}
usleep(10000);
for (j = 0; j < 10; j++) {
/* sleep 20 milliseconds */
usleep(20000);
nbytes = read(fd0, &sample1, sizeof(sample1));
/* Handle unexpected return values */
if (nbytes < 0) {
errval = errno;
if (errval != EINTR) {
message("reading /dev/adc0 failed: %d\n", errval);
errval = 3;
goto errout_with_dev;
}
message("\tinterrupted read..\n");
} else if (nbytes == 0) {
message("\tno data read, ignoring.\n");
ret = ERROR;
for (unsigned j = 0; j < channels; j++) {
printf("%d: %u ", data[j].am_channel, data[j].am_data);
}
/* Print the sample data on successful return */
else {
if (nbytes != sizeof(sample1)) {
message("\tsample 1 size %d is not matching struct size %d, ignoring\n",
nbytes, sizeof(sample1));
ret = ERROR;
} else {
message("CYCLE %d:\n", j);
message("channel: %d value: %d\n",
(int)sample1.am_channel1, sample1.am_data1);
message("channel: %d value: %d\n",
(int)sample1.am_channel2, sample1.am_data2);
message("channel: %d value: %d\n",
(int)sample1.am_channel3, sample1.am_data3);
message("channel: %d value: %d\n",
(int)sample1.am_channel4, sample1.am_data4);
}
}
fflush(stdout);
printf("\n");
usleep(150000);
}
message("\t ADC test successful.\n");
errout_with_dev:
if (fd0 != 0) close(fd0);
if (fd != 0) close(fd);
return ret;
return OK;
}

2
apps/px4/tests/test_bson.c
View File

@ -240,5 +240,5 @@ test_bson(int argc, char *argv[])
decode(&decoder);
free(buf);
exit(0);
return OK;
}

173
apps/px4/tests/test_jig_voltages.c
View File

@ -52,7 +52,8 @@
#include "tests.h"
#include <nuttx/analog/adc.h>
#include <drivers/drv_adc.h>
#include <systemlib/err.h>
/****************************************************************************
* Pre-processor Definitions
@ -89,129 +90,79 @@
int test_jig_voltages(int argc, char *argv[])
{
int fd0 = 0;
int ret = OK;
const int nchannels = 4;
int fd = open(ADC_DEVICE_PATH, O_RDONLY);
int ret = OK;
struct adc_msg4_s
{
uint8_t am_channel1; /* The 8-bit ADC Channel */
int32_t am_data1; /* ADC convert result (4 bytes) */
uint8_t am_channel2; /* The 8-bit ADC Channel */
int32_t am_data2; /* ADC convert result (4 bytes) */
uint8_t am_channel3; /* The 8-bit ADC Channel */
int32_t am_data3; /* ADC convert result (4 bytes) */
uint8_t am_channel4; /* The 8-bit ADC Channel */
int32_t am_data4; /* ADC convert result (4 bytes) */
}__attribute__((__packed__));;
struct adc_msg4_s sample1[4];
size_t readsize;
ssize_t nbytes;
int i = 0;
int j = 0;
int errval;
char name[11];
sprintf(name, "/dev/adc%d", j);
fd0 = open(name, O_RDONLY | O_NONBLOCK);
if (fd0 < 0)
{
printf("ADC: %s open fail\n", name);
return ERROR;
} else {
printf("Opened %s successfully\n", name);
if (fd < 0) {
warnx("can't open ADC device");
return 1;
}
/* make space for a maximum of eight channels */
struct adc_msg_s data[8];
/* read all channels available */
ssize_t count = read(fd, data, sizeof(data));
if (count < 0) {
close(fd);
warnx("can't read from ADC driver. Forgot 'adc start' command?");
return 1;
}
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");
warnx("\t ADC operational.\n");
/* Expected values */
int16_t expected_min[] = {2700, 2700, 2200, 2000};
int16_t expected_max[] = {3000, 3000, 2500, 2200};
char* check_res[nchannels];
char *check_res[channels];
/* first adc read round */
readsize = 4 * sizeof(struct adc_msg_s);
if (channels < 4) {
close(fd);
warnx("not all four test channels available, aborting.");
return 1;
/* Empty all buffers */
do {
nbytes = read(fd0, sample1, readsize);
} else {
/* Check values */
check_res[0] = (expected_min[0] < data[0].am_data && expected_max[0] > data[0].am_data) ? "OK" : "FAIL";
check_res[1] = (expected_min[1] < data[1].am_data && expected_max[1] > data[1].am_data) ? "OK" : "FAIL";
check_res[2] = (expected_min[2] < data[2].am_data && expected_max[2] > data[2].am_data) ? "OK" : "FAIL";
check_res[3] = (expected_min[3] < data[3].am_data && expected_max[3] > data[3].am_data) ? "OK" : "FAIL";
/* Accumulate result */
ret += (expected_min[0] > data[0].am_data || expected_max[0] < data[0].am_data) ? 1 : 0;
ret += (expected_min[1] > data[1].am_data || expected_max[1] < data[1].am_data) ? 1 : 0;
ret += (expected_min[2] > data[2].am_data || expected_max[2] < data[2].am_data) ? 1 : 0;
ret += (expected_min[3] > data[3].am_data || expected_max[3] < data[3].am_data) ? 1 : 0;
message("Sample:");
message("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
data[0].am_channel, (int)(data[0].am_data), expected_min[0], expected_max[0], check_res[0]);
message("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
data[1].am_channel, (int)(data[1].am_data), expected_min[1], expected_max[1], check_res[1]);
message("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
data[2].am_channel, (int)(data[2].am_data), expected_min[2], expected_max[2], check_res[2]);
message("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
data[3].am_channel, (int)(data[3].am_data), expected_min[3], expected_max[3], check_res[3]);
if (ret != OK) {
printf("\t JIG voltages test FAILED. Some channels where out of allowed range. Check supply voltages.\n");
goto errout_with_dev;
}
}
while (nbytes > 0);
up_udelay(20000);//microseconds
/* Take measurements */
nbytes = read(fd0, sample1, readsize);
printf("\t JIG voltages test successful.\n");
/* Handle unexpected return values */
errout_with_dev:
if (nbytes <= 0)
{
errval = errno;
if (errval != EINTR)
{
message("read %s failed: %d\n",
name, errval);
errval = 3;
goto errout_with_dev;
}
message("\tInterrupted read...\n");
}
else if (nbytes == 0)
{
message("\tNo data read, Ignoring\n");
}
/* Print the sample data on successful return */
else
{
int nsamples = nbytes / sizeof(struct adc_msg_s);
if (nsamples * sizeof(struct adc_msg_s) != nbytes)
{
message("\tread size=%d is not a multiple of sample size=%d, Ignoring\n",
nbytes, sizeof(struct adc_msg_s));
}
else
{
/* Check values */
check_res[0] = (expected_min[0] < sample1[i].am_data1 && expected_max[0] > sample1[i].am_data1) ? "OK" : "FAIL";
check_res[1] = (expected_min[1] < sample1[i].am_data2 && expected_max[1] > sample1[i].am_data2) ? "OK" : "FAIL";
check_res[2] = (expected_min[2] < sample1[i].am_data3 && expected_max[2] > sample1[i].am_data3) ? "OK" : "FAIL";
check_res[3] = (expected_min[3] < sample1[i].am_data4 && expected_max[3] > sample1[i].am_data4) ? "OK" : "FAIL";
/* Accumulate result */
ret += (expected_min[0] > sample1[i].am_data1 || expected_max[0] < sample1[i].am_data1) ? 1 : 0;
// XXX Chan 11 not connected on test setup
//ret += (expected_min[1] > sample1[i].am_data2 || expected_max[1] < sample1[i].am_data2) ? 1 : 0;
ret += (expected_min[2] > sample1[i].am_data3 || expected_max[2] < sample1[i].am_data3) ? 1 : 0;
ret += (expected_min[3] > sample1[i].am_data4 || expected_max[3] < sample1[i].am_data4) ? 1 : 0;
message("Sample:");
message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
i, sample1[i].am_channel1, sample1[i].am_data1, expected_min[0], expected_max[0], check_res[0]);
message("Sample:");
message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
i, sample1[i].am_channel2, sample1[i].am_data2, expected_min[1], expected_max[1], check_res[1]);
message("Sample:");
message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
i, sample1[i].am_channel3, sample1[i].am_data3, expected_min[2], expected_max[2], check_res[2]);
message("Sample:");
message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n",
i, sample1[i].am_channel4, sample1[i].am_data4, expected_min[3], expected_max[3], check_res[3]);
if (ret != OK) {
printf("\t ADC test FAILED. Some channels where out of allowed range. Check supply voltages.\n");
goto errout_with_dev;
}
}
}
printf("\t ADC test successful.\n");
errout_with_dev:
if (fd0 != 0) close(fd0);
if (fd != 0) close(fd);
return ret;
}

3
apps/px4/tests/test_sleep.c
View File

@ -90,9 +90,8 @@ int test_sleep(int argc, char *argv[])
printf("\t %d 100ms sleeps\n", nsleeps);
fflush(stdout);
for (int i = 0; i < nsleeps; i++) {
for (unsigned int i = 0; i < nsleeps; i++) {
usleep(100000);
//printf("\ttick\n");
}
printf("\t Sleep test successful.\n");

39
apps/px4/tests/tests_main.c
View File

@ -78,11 +78,10 @@ static int test_jig(int argc, char *argv[]);
* Private Data
****************************************************************************/
struct {
const struct {
const char *name;
int (* fn)(int argc, char *argv[]);
unsigned options;
int passed;
#define OPT_NOHELP (1<<0)
#define OPT_NOALLTEST (1<<1)
#define OPT_NOJIGTEST (1<<2)
@ -107,9 +106,9 @@ struct {
{"perf", test_perf, OPT_NOJIGTEST, 0},
{"all", test_all, OPT_NOALLTEST | OPT_NOJIGTEST, 0},
{"jig", test_jig, OPT_NOJIGTEST | OPT_NOALLTEST, 0},
{"param", test_param, 0, 0},
{"bson", test_bson, 0, 0},
{"file", test_file, 0, 0},
{"param", test_param, OPT_NOJIGTEST, 0},
{"bson", test_bson, OPT_NOJIGTEST, 0},
{"file", test_file, OPT_NOJIGTEST, 0},
{"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST, 0},
{NULL, NULL, 0, 0}
};
@ -133,6 +132,11 @@ test_all(int argc, char *argv[])
unsigned i;
char *args[2] = {"all", NULL};
unsigned int failcount = 0;
unsigned int testscount = 0;
/* store test results */
bool passed[(sizeof(tests) / sizeof(tests[0]))];
memset(&passed, 0, sizeof(passed));
printf("\nRunning all tests...\n\n");
@ -149,10 +153,12 @@ test_all(int argc, char *argv[])
failcount++;
} else {
tests[i].passed = 1;
passed[i] = true;
printf(" [%s] \t\t\tPASS\n", tests[i].name);
fflush(stdout);
}
testscount++;
}
}
@ -175,7 +181,7 @@ test_all(int argc, char *argv[])
printf(" \\ \\_\\ \\_\\ \\ \\_____\\ \\ \\_____\\ \\ \\_____\\ \\ \\_\\ \\_\\ \n");
printf(" \\/_/\\/_/ \\/_____/ \\/_____/ \\/_____/ \\/_/\\/_/ \n");
printf("\n");
printf(" All tests passed (%d of %d)\n", i, i);
printf(" All tests passed (%d of %d)\n", testscount, testscount);
} else {
printf(" ______ ______ __ __ \n");
@ -184,7 +190,7 @@ test_all(int argc, char *argv[])
printf(" \\ \\_\\ \\ \\_\\ \\_\\ \\ \\_\\ \\ \\_____\\ \n");
printf(" \\/_/ \\/_/\\/_/ \\/_/ \\/_____/ \n");
printf("\n");
printf(" Some tests failed (%d of %d)\n", failcount, i);
printf(" Some tests failed (%d of %d)\n", failcount, testscount);
}
printf("\n");
@ -195,7 +201,7 @@ test_all(int argc, char *argv[])
unsigned int k;
for (k = 0; k < i; k++) {
if ((tests[k].passed == 0) && !(tests[k].options & OPT_NOALLTEST)) {
if (!passed[k] && !(tests[k].options & OPT_NOALLTEST)) {
printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
}
}
@ -242,6 +248,11 @@ int test_jig(int argc, char *argv[])
unsigned i;
char *args[2] = {"jig", NULL};
unsigned int failcount = 0;
unsigned int testscount = 0;
/* store test results */
bool passed[(sizeof(tests) / sizeof(tests[0]))];
memset(&passed, 0, sizeof(passed));
printf("\nRunning all tests...\n\n");
for (i = 0; tests[i].name; i++) {
@ -255,10 +266,12 @@ int test_jig(int argc, char *argv[])
fflush(stderr);
failcount++;
} else {
tests[i].passed = 1;
passed[i] = true;
printf(" [%s] \t\t\tPASS\n", tests[i].name);
fflush(stdout);
}
testscount++;
}
}
@ -279,7 +292,7 @@ int test_jig(int argc, char *argv[])
printf(" \\ \\_\\ \\_\\ \\ \\_____\\ \\ \\_____\\ \\ \\_____\\ \\ \\_\\ \\_\\ \n");
printf(" \\/_/\\/_/ \\/_____/ \\/_____/ \\/_____/ \\/_/\\/_/ \n");
printf("\n");
printf(" All tests passed (%d of %d)\n", i, i);
printf(" All tests passed (%d of %d)\n", testscount, testscount);
} else {
printf(" ______ ______ __ __ \n");
printf(" /\\ ___\\ /\\ __ \\ /\\ \\ /\\ \\ \n");
@ -287,7 +300,7 @@ int test_jig(int argc, char *argv[])
printf(" \\ \\_\\ \\ \\_\\ \\_\\ \\ \\_\\ \\ \\_____\\ \n");
printf(" \\/_/ \\/_/\\/_/ \\/_/ \\/_____/ \n");
printf("\n");
printf(" Some tests failed (%d of %d)\n", failcount, i);
printf(" Some tests failed (%d of %d)\n", failcount, testscount);
}
printf("\n");
@ -296,7 +309,7 @@ int test_jig(int argc, char *argv[])
unsigned int k;
for (k = 0; k < i; k++)
{
if ((tests[k].passed == 0) && !(tests[k].options & OPT_NOJIGTEST))
if (!passed[k] && !(tests[k].options & OPT_NOJIGTEST))
{
printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name);
}

159
apps/sensors/sensors.cpp
View File

@ -43,7 +43,6 @@
#include <fcntl.h>
#include <poll.h>
#include <nuttx/analog/adc.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
@ -52,13 +51,15 @@
#include <errno.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <nuttx/analog/adc.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_adc.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@ -87,7 +88,7 @@
#define BARO_HEALTH_COUNTER_LIMIT_OK 5
#define ADC_HEALTH_COUNTER_LIMIT_OK 5
#define ADC_BATTERY_VOLATGE_CHANNEL 10
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
#define BAT_VOL_INITIAL 12.f
#define BAT_VOL_LOWPASS_1 0.99f
@ -108,8 +109,8 @@ extern "C" __EXPORT int sensors_main(int argc, char *argv[]);
class Sensors
{
public:
/**
* Constructor
/**
* Constructor
*/
Sensors();
@ -125,7 +126,7 @@ public:
*/
int start();
private:
private:
static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */
#if CONFIG_HRT_PPM
@ -233,7 +234,7 @@ private:
param_t rc_map_pitch;
param_t rc_map_yaw;
param_t rc_map_throttle;
param_t rc_map_manual_override_sw;
param_t rc_map_auto_mode_sw;
@ -373,7 +374,7 @@ Sensors::Sensors() :
_hil_enabled(false),
_publishing(true),
/* subscriptions */
/* subscriptions */
_gyro_sub(-1),
_accel_sub(-1),
_mag_sub(-1),
@ -383,13 +384,13 @@ Sensors::Sensors() :
_params_sub(-1),
_manual_control_sub(-1),
/* publications */
/* publications */
_sensor_pub(-1),
_manual_control_pub(-1),
_rc_pub(-1),
_battery_pub(-1),
/* performance counters */
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
{
@ -487,6 +488,7 @@ Sensors::~Sensors()
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
usleep(20000);
@ -513,15 +515,19 @@ Sensors::parameters_update()
if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) {
warnx("Failed getting min for chan %d", i);
}
if (param_get(_parameter_handles.trim[i], &(_parameters.trim[i])) != OK) {
warnx("Failed getting trim for chan %d", i);
}
if (param_get(_parameter_handles.max[i], &(_parameters.max[i])) != OK) {
warnx("Failed getting max for chan %d", i);
}
if (param_get(_parameter_handles.rev[i], &(_parameters.rev[i])) != OK) {
warnx("Failed getting rev for chan %d", i);
}
if (param_get(_parameter_handles.dz[i], &(_parameters.dz[i])) != OK) {
warnx("Failed getting dead zone for chan %d", i);
}
@ -530,8 +536,8 @@ Sensors::parameters_update()
/* handle blowup in the scaling factor calculation */
if (!isfinite(_parameters.scaling_factor[i]) ||
_parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f ||
_parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) {
_parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f ||
_parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) {
/* scaling factors do not make sense, lock them down */
_parameters.scaling_factor[i] = 0;
@ -553,18 +559,23 @@ Sensors::parameters_update()
if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
warnx("Failed getting roll chan index");
}
if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) {
warnx("Failed getting pitch chan index");
}
if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) {
warnx("Failed getting yaw chan index");
}
if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) {
warnx("Failed getting throttle chan index");
}
if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) {
warnx("Failed getting override sw chan index");
}
if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) {
warnx("Failed getting auto mode sw chan index");
}
@ -576,12 +587,15 @@ Sensors::parameters_update()
if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) {
warnx("Failed getting manual mode sw chan index");
}
if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) {
warnx("Failed getting rtl sw chan index");
}
if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) {
warnx("Failed getting sas mode sw chan index");
}
if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
warnx("Failed getting offboard control mode sw chan index");
}
@ -589,15 +603,19 @@ Sensors::parameters_update()
if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) {
warnx("Failed getting mode aux 1 index");
}
if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) {
warnx("Failed getting mode aux 2 index");
}
if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) {
warnx("Failed getting mode aux 3 index");
}
if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) {
warnx("Failed getting mode aux 4 index");
}
if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) {
warnx("Failed getting mode aux 5 index");
}
@ -605,12 +623,15 @@ Sensors::parameters_update()
if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) {
warnx("Failed getting rc scaling for roll");
}
if (param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)) != OK) {
warnx("Failed getting rc scaling for pitch");
}
if (param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)) != OK) {
warnx("Failed getting rc scaling for yaw");
}
if (param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)) != OK) {
warnx("Failed getting rc scaling for flaps");
}
@ -673,9 +694,11 @@ Sensors::accel_init()
int fd;
fd = open(ACCEL_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", ACCEL_DEVICE_PATH);
errx(1, "FATAL: no accelerometer found");
} else {
/* set the accel internal sampling rate up to at leat 500Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 500);
@ -694,9 +717,11 @@ Sensors::gyro_init()
int fd;
fd = open(GYRO_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", GYRO_DEVICE_PATH);
errx(1, "FATAL: no gyro found");
} else {
/* set the gyro internal sampling rate up to at leat 500Hz */
ioctl(fd, GYROIOCSSAMPLERATE, 500);
@ -715,6 +740,7 @@ Sensors::mag_init()
int fd;
fd = open(MAG_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", MAG_DEVICE_PATH);
errx(1, "FATAL: no magnetometer found");
@ -735,6 +761,7 @@ Sensors::baro_init()
int fd;
fd = open(BARO_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", BARO_DEVICE_PATH);
warnx("No barometer found, ignoring");
@ -750,9 +777,10 @@ void
Sensors::adc_init()
{
_fd_adc = open("/dev/adc0", O_RDONLY | O_NONBLOCK);
_fd_adc = open(ADC_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
if (_fd_adc < 0) {
warnx("/dev/adc0");
warn(ADC_DEVICE_PATH);
warnx("FATAL: no ADC found");
}
}
@ -821,7 +849,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
raw.magnetometer_raw[0] = mag_report.x_raw;
raw.magnetometer_raw[1] = mag_report.y_raw;
raw.magnetometer_raw[2] = mag_report.z_raw;
raw.magnetometer_counter++;
}
}
@ -853,6 +881,7 @@ Sensors::vehicle_status_poll()
/* Check HIL state if vehicle status has changed */
orb_check(_vstatus_sub, &vstatus_updated);
if (vstatus_updated) {
orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus);
@ -880,8 +909,7 @@ Sensors::parameter_update_poll(bool forced)
/* Check if any parameter has changed */
orb_check(_params_sub, &param_updated);
if (param_updated || forced)
{
if (param_updated || forced) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
@ -891,7 +919,7 @@ Sensors::parameter_update_poll(bool forced)
/* update sensor offsets */
int fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale = {
struct gyro_scale gscale = {
_parameters.gyro_offset[0],
1.0f,
_parameters.gyro_offset[1],
@ -899,8 +927,10 @@ Sensors::parameter_update_poll(bool forced)
_parameters.gyro_offset[2],
1.0f,
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
warn("WARNING: failed to set scale / offsets for gyro");
close(fd);
fd = open(ACCEL_DEVICE_PATH, 0);
@ -912,8 +942,10 @@ Sensors::parameter_update_poll(bool forced)
_parameters.accel_offset[2],
_parameters.accel_scale[2],
};
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
warn("WARNING: failed to set scale / offsets for accel");
close(fd);
fd = open(MAG_DEVICE_PATH, 0);
@ -925,62 +957,68 @@ Sensors::parameter_update_poll(bool forced)
_parameters.mag_offset[2],
_parameters.mag_scale[2],
};
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
warn("WARNING: failed to set scale / offsets for mag");
close(fd);
#if 0
printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor*10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]);
printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor*10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]);
printf("MAN: %d %d\n", (int)(_rc.chan[0].scaled*100), (int)(_rc.chan[1].scaled*100));
printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor * 10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]);
printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor * 10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]);
printf("MAN: %d %d\n", (int)(_rc.chan[0].scaled * 100), (int)(_rc.chan[1].scaled * 100));
fflush(stdout);
usleep(5000);
#endif
}
}
}
void
Sensors::adc_poll(struct sensor_combined_s &raw)
{
#pragma pack(push,1)
struct adc_msg4_s {
uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */
int32_t am_data1; /**< ADC convert result 1 (4 bytes) */
uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */
int32_t am_data2; /**< ADC convert result 2 (4 bytes) */
uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */
int32_t am_data3; /**< ADC convert result 3 (4 bytes) */
uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */
int32_t am_data4; /**< ADC convert result 4 (4 bytes) */
} buf_adc;
#pragma pack(pop)
/* rate limit to 100 Hz */
if (hrt_absolute_time() - _last_adc >= 10000) {
read(_fd_adc, &buf_adc, sizeof(buf_adc));
/* make space for a maximum of eight channels */
struct adc_msg_s buf_adc[8];
/* read all channels available */
int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc));
if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) {
/* Voltage in volts */
float voltage = (buf_adc.am_data1 * _parameters.battery_voltage_scaling);
/* look for battery channel */
if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
_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;
if (ret >= sizeof(buf_adc[0]) && ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
/* Voltage in volts */
float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
/* 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);
if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
/* one-time initialization of low-pass value to avoid long init delays */
if (_battery_status.voltage_v < 3.0f) {
_battery_status.voltage_v = voltage;
}
_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++;
raw.battery_voltage_counter++;
_last_adc = hrt_absolute_time();
break;
}
}
_last_adc = hrt_absolute_time();
}
}
@ -1012,6 +1050,7 @@ Sensors::ppm_poll()
/* publish to object request broker */
if (rc_input_pub <= 0) {
rc_input_pub = orb_advertise(ORB_ID(input_rc), &raw);
} else {
orb_publish(ORB_ID(input_rc), rc_input_pub, &raw);
}
@ -1052,6 +1091,7 @@ Sensors::ppm_poll()
return;
unsigned channel_limit = rc_input.channel_count;
if (channel_limit > _rc_max_chan_count)
channel_limit = _rc_max_chan_count;
@ -1064,10 +1104,11 @@ Sensors::ppm_poll()
/* scale around the mid point differently for lower and upper range */
if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) {
_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]);
} else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
/* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */
_rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i]));
} else {
/* in the configured dead zone, output zero */
_rc.chan[i].scaled = 0.0f;
@ -1078,6 +1119,7 @@ Sensors::ppm_poll()
if ((int)_parameters.rev[i] == -1) {
_rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled;
}
} else {
_rc.chan[i].scaled *= _parameters.rev[i];
}
@ -1103,7 +1145,9 @@ Sensors::ppm_poll()
manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled);
/* throttle input */
manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
/* scale output */
@ -1175,6 +1219,7 @@ Sensors::ppm_poll()
/* check if ready for publishing */
if (_rc_pub > 0) {
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
} else {
/* advertise the rc topic */
_rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc);
@ -1183,6 +1228,7 @@ Sensors::ppm_poll()
/* check if ready for publishing */
if (_manual_control_pub > 0) {
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
} else {
_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
}
@ -1260,7 +1306,7 @@ Sensors::task_main()
fds[0].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
@ -1329,6 +1375,7 @@ Sensors::start()
warn("task start failed");
return -errno;
}
return OK;
}
@ -1343,6 +1390,7 @@ int sensors_main(int argc, char *argv[])
errx(1, "sensors task already running");
sensors::g_sensors = new Sensors;
if (sensors::g_sensors == nullptr)
errx(1, "sensors task alloc failed");
@ -1351,12 +1399,14 @@ int sensors_main(int argc, char *argv[])
sensors::g_sensors = nullptr;
err(1, "sensors task start failed");
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
if (sensors::g_sensors == nullptr)
errx(1, "sensors task not running");
delete sensors::g_sensors;
sensors::g_sensors = nullptr;
exit(0);
@ -1365,6 +1415,7 @@ int sensors_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (sensors::g_sensors) {
errx(0, "task is running");
} else {
errx(1, "task is not running");
}