forked from Archive/PX4-Autopilot
Major formatting/whitespace cleanup
This commit is contained in:
parent
34f99c7dca
commit
2fc1032069
|
@ -289,6 +289,7 @@ BMA180::init()
|
||||||
_num_reports = 2;
|
_num_reports = 2;
|
||||||
_oldest_report = _next_report = 0;
|
_oldest_report = _next_report = 0;
|
||||||
_reports = new struct accel_report[_num_reports];
|
_reports = new struct accel_report[_num_reports];
|
||||||
|
|
||||||
if (_reports == nullptr)
|
if (_reports == nullptr)
|
||||||
goto out;
|
goto out;
|
||||||
|
|
||||||
|
@ -321,13 +322,16 @@ BMA180::init()
|
||||||
modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
|
modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
|
||||||
|
|
||||||
if (set_range(4)) warnx("Failed setting range");
|
if (set_range(4)) warnx("Failed setting range");
|
||||||
|
|
||||||
if (set_lowpass(75)) warnx("Failed setting lowpass");
|
if (set_lowpass(75)) warnx("Failed setting lowpass");
|
||||||
|
|
||||||
if (read_reg(ADDR_CHIP_ID) == CHIP_ID) {
|
if (read_reg(ADDR_CHIP_ID) == CHIP_ID) {
|
||||||
ret = OK;
|
ret = OK;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
ret = ERROR;
|
ret = ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
out:
|
out:
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
@ -441,6 +445,7 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
case SENSORIOCGPOLLRATE:
|
case SENSORIOCGPOLLRATE:
|
||||||
if (_call_interval == 0)
|
if (_call_interval == 0)
|
||||||
return SENSOR_POLLRATE_MANUAL;
|
return SENSOR_POLLRATE_MANUAL;
|
||||||
|
|
||||||
return 1000000 / _call_interval;
|
return 1000000 / _call_interval;
|
||||||
|
|
||||||
case SENSORIOCSQUEUEDEPTH: {
|
case SENSORIOCSQUEUEDEPTH: {
|
||||||
|
@ -468,7 +473,7 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
}
|
}
|
||||||
|
|
||||||
case SENSORIOCGQUEUEDEPTH:
|
case SENSORIOCGQUEUEDEPTH:
|
||||||
return _num_reports -1;
|
return _num_reports - 1;
|
||||||
|
|
||||||
case SENSORIOCRESET:
|
case SENSORIOCRESET:
|
||||||
/* XXX implement */
|
/* XXX implement */
|
||||||
|
@ -488,12 +493,12 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
|
|
||||||
case ACCELIOCSSCALE:
|
case ACCELIOCSSCALE:
|
||||||
/* copy scale in */
|
/* copy scale in */
|
||||||
memcpy(&_accel_scale, (struct accel_scale*) arg, sizeof(_accel_scale));
|
memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale));
|
||||||
return OK;
|
return OK;
|
||||||
|
|
||||||
case ACCELIOCGSCALE:
|
case ACCELIOCGSCALE:
|
||||||
/* copy scale out */
|
/* copy scale out */
|
||||||
memcpy((struct accel_scale*) arg, &_accel_scale, sizeof(_accel_scale));
|
memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
|
||||||
return OK;
|
return OK;
|
||||||
|
|
||||||
case ACCELIOCSRANGE:
|
case ACCELIOCSRANGE:
|
||||||
|
@ -549,24 +554,30 @@ BMA180::set_range(unsigned max_g)
|
||||||
|
|
||||||
if (max_g == 0)
|
if (max_g == 0)
|
||||||
max_g = 16;
|
max_g = 16;
|
||||||
|
|
||||||
if (max_g > 16)
|
if (max_g > 16)
|
||||||
return -ERANGE;
|
return -ERANGE;
|
||||||
|
|
||||||
if (max_g <= 2) {
|
if (max_g <= 2) {
|
||||||
_current_range = 2;
|
_current_range = 2;
|
||||||
rangebits = OFFSET_LSB1_RANGE_2G;
|
rangebits = OFFSET_LSB1_RANGE_2G;
|
||||||
|
|
||||||
} else if (max_g <= 3) {
|
} else if (max_g <= 3) {
|
||||||
_current_range = 3;
|
_current_range = 3;
|
||||||
rangebits = OFFSET_LSB1_RANGE_3G;
|
rangebits = OFFSET_LSB1_RANGE_3G;
|
||||||
|
|
||||||
} else if (max_g <= 4) {
|
} else if (max_g <= 4) {
|
||||||
_current_range = 4;
|
_current_range = 4;
|
||||||
rangebits = OFFSET_LSB1_RANGE_4G;
|
rangebits = OFFSET_LSB1_RANGE_4G;
|
||||||
|
|
||||||
} else if (max_g <= 8) {
|
} else if (max_g <= 8) {
|
||||||
_current_range = 8;
|
_current_range = 8;
|
||||||
rangebits = OFFSET_LSB1_RANGE_8G;
|
rangebits = OFFSET_LSB1_RANGE_8G;
|
||||||
|
|
||||||
} else if (max_g <= 16) {
|
} else if (max_g <= 16) {
|
||||||
_current_range = 16;
|
_current_range = 16;
|
||||||
rangebits = OFFSET_LSB1_RANGE_16G;
|
rangebits = OFFSET_LSB1_RANGE_16G;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
return -EINVAL;
|
return -EINVAL;
|
||||||
}
|
}
|
||||||
|
@ -586,7 +597,7 @@ BMA180::set_range(unsigned max_g)
|
||||||
|
|
||||||
/* check if wanted value is now in register */
|
/* check if wanted value is now in register */
|
||||||
return !((read_reg(ADDR_OFFSET_LSB1) & OFFSET_LSB1_RANGE_MASK) ==
|
return !((read_reg(ADDR_OFFSET_LSB1) & OFFSET_LSB1_RANGE_MASK) ==
|
||||||
(OFFSET_LSB1_RANGE_MASK & rangebits));
|
(OFFSET_LSB1_RANGE_MASK & rangebits));
|
||||||
}
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
|
@ -633,7 +644,7 @@ BMA180::set_lowpass(unsigned frequency)
|
||||||
|
|
||||||
/* check if wanted value is now in register */
|
/* check if wanted value is now in register */
|
||||||
return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) ==
|
return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) ==
|
||||||
(BW_TCS_BW_MASK & bwbits));
|
(BW_TCS_BW_MASK & bwbits));
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -703,9 +714,9 @@ BMA180::measure()
|
||||||
* perform only the axis assignment here.
|
* perform only the axis assignment here.
|
||||||
* Two non-value bits are discarded directly
|
* Two non-value bits are discarded directly
|
||||||
*/
|
*/
|
||||||
report->y_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+1)) << 8) | (read_reg(ADDR_ACC_X_LSB));// XXX PX4DEV raw_report.x;
|
report->y_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 1)) << 8) | (read_reg(ADDR_ACC_X_LSB)); // XXX PX4DEV raw_report.x;
|
||||||
report->x_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+3)) << 8) | (read_reg(ADDR_ACC_X_LSB+2));// XXX PX4DEV raw_report.y;
|
report->x_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 3)) << 8) | (read_reg(ADDR_ACC_X_LSB + 2)); // XXX PX4DEV raw_report.y;
|
||||||
report->z_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+5)) << 8) | (read_reg(ADDR_ACC_X_LSB+4));// XXX PX4DEV raw_report.z;
|
report->z_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 5)) << 8) | (read_reg(ADDR_ACC_X_LSB + 4)); // XXX PX4DEV raw_report.z;
|
||||||
|
|
||||||
/* discard two non-value bits in the 16 bit measurement */
|
/* discard two non-value bits in the 16 bit measurement */
|
||||||
report->x_raw = (report->x_raw >> 2);
|
report->x_raw = (report->x_raw >> 2);
|
||||||
|
@ -781,17 +792,21 @@ start()
|
||||||
|
|
||||||
/* set the poll rate to default, starts automatic data collection */
|
/* set the poll rate to default, starts automatic data collection */
|
||||||
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
goto fail;
|
goto fail;
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||||
goto fail;
|
goto fail;
|
||||||
|
|
||||||
exit(0);
|
exit(0);
|
||||||
fail:
|
fail:
|
||||||
|
|
||||||
if (g_dev != nullptr) {
|
if (g_dev != nullptr) {
|
||||||
delete g_dev;
|
delete g_dev;
|
||||||
g_dev = nullptr;
|
g_dev = nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
errx(1, "driver start failed");
|
errx(1, "driver start failed");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -809,16 +824,18 @@ test()
|
||||||
|
|
||||||
/* get the driver */
|
/* get the driver */
|
||||||
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
err(1, "%s open failed (try 'bma180 start' if the driver is not running)",
|
err(1, "%s open failed (try 'bma180 start' if the driver is not running)",
|
||||||
ACCEL_DEVICE_PATH);
|
ACCEL_DEVICE_PATH);
|
||||||
|
|
||||||
/* reset to manual polling */
|
/* reset to manual polling */
|
||||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
|
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
|
||||||
err(1, "reset to manual polling");
|
err(1, "reset to manual polling");
|
||||||
|
|
||||||
/* do a simple demand read */
|
/* do a simple demand read */
|
||||||
sz = read(fd, &a_report, sizeof(a_report));
|
sz = read(fd, &a_report, sizeof(a_report));
|
||||||
|
|
||||||
if (sz != sizeof(a_report))
|
if (sz != sizeof(a_report))
|
||||||
err(1, "immediate acc read failed");
|
err(1, "immediate acc read failed");
|
||||||
|
|
||||||
|
@ -831,7 +848,7 @@ test()
|
||||||
warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
|
warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
|
||||||
warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
|
warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
|
||||||
warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
|
warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
|
||||||
(double)(a_report.range_m_s2 / 9.81f));
|
(double)(a_report.range_m_s2 / 9.81f));
|
||||||
|
|
||||||
/* XXX add poll-rate tests here too */
|
/* XXX add poll-rate tests here too */
|
||||||
|
|
||||||
|
@ -846,10 +863,13 @@ void
|
||||||
reset()
|
reset()
|
||||||
{
|
{
|
||||||
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
err(1, "failed ");
|
err(1, "failed ");
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
|
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
|
||||||
err(1, "driver reset failed");
|
err(1, "driver reset failed");
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||||
err(1, "driver poll restart failed");
|
err(1, "driver poll restart failed");
|
||||||
|
|
||||||
|
|
|
@ -99,25 +99,22 @@ int adc_devinit(void)
|
||||||
|
|
||||||
/* Check if we have already initialized */
|
/* Check if we have already initialized */
|
||||||
|
|
||||||
if (!initialized)
|
if (!initialized) {
|
||||||
{
|
|
||||||
char name[11];
|
char name[11];
|
||||||
|
|
||||||
for (i = 0; i < ADC3_NCHANNELS; i++)
|
for (i = 0; i < ADC3_NCHANNELS; i++) {
|
||||||
{
|
stm32_configgpio(g_pinlist[i]);
|
||||||
stm32_configgpio(g_pinlist[i]);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
for (i = 0; i < 1; i++)
|
for (i = 0; i < 1; i++) {
|
||||||
{
|
|
||||||
/* Configure the pins as analog inputs for the selected channels */
|
/* Configure the pins as analog inputs for the selected channels */
|
||||||
//stm32_configgpio(g_pinlist[i]);
|
//stm32_configgpio(g_pinlist[i]);
|
||||||
|
|
||||||
/* Call stm32_adcinitialize() to get an instance of the ADC interface */
|
/* Call stm32_adcinitialize() to get an instance of the ADC interface */
|
||||||
//multiple channels only supported with dma!
|
//multiple channels only supported with dma!
|
||||||
adc[i] = stm32_adcinitialize(3, (g_chanlist), 4);
|
adc[i] = stm32_adcinitialize(3, (g_chanlist), 4);
|
||||||
if (adc == NULL)
|
|
||||||
{
|
if (adc == NULL) {
|
||||||
adbg("ERROR: Failed to get ADC interface\n");
|
adbg("ERROR: Failed to get ADC interface\n");
|
||||||
return -ENODEV;
|
return -ENODEV;
|
||||||
}
|
}
|
||||||
|
@ -126,8 +123,8 @@ int adc_devinit(void)
|
||||||
/* Register the ADC driver at "/dev/adc0" */
|
/* Register the ADC driver at "/dev/adc0" */
|
||||||
sprintf(name, "/dev/adc%d", i);
|
sprintf(name, "/dev/adc%d", i);
|
||||||
ret = adc_register(name, adc[i]);
|
ret = adc_register(name, adc[i]);
|
||||||
if (ret < 0)
|
|
||||||
{
|
if (ret < 0) {
|
||||||
adbg("adc_register failed for adc %s: %d\n", name, ret);
|
adbg("adc_register failed for adc %s: %d\n", name, ret);
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
|
@ -107,36 +107,35 @@
|
||||||
|
|
||||||
int can_devinit(void)
|
int can_devinit(void)
|
||||||
{
|
{
|
||||||
static bool initialized = false;
|
static bool initialized = false;
|
||||||
struct can_dev_s *can;
|
struct can_dev_s *can;
|
||||||
int ret;
|
int ret;
|
||||||
|
|
||||||
/* Check if we have already initialized */
|
/* Check if we have already initialized */
|
||||||
|
|
||||||
if (!initialized)
|
if (!initialized) {
|
||||||
{
|
/* Call stm32_caninitialize() to get an instance of the CAN interface */
|
||||||
/* Call stm32_caninitialize() to get an instance of the CAN interface */
|
|
||||||
|
|
||||||
can = stm32_caninitialize(CAN_PORT);
|
can = stm32_caninitialize(CAN_PORT);
|
||||||
if (can == NULL)
|
|
||||||
{
|
|
||||||
candbg("ERROR: Failed to get CAN interface\n");
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Register the CAN driver at "/dev/can0" */
|
if (can == NULL) {
|
||||||
|
candbg("ERROR: Failed to get CAN interface\n");
|
||||||
|
return -ENODEV;
|
||||||
|
}
|
||||||
|
|
||||||
ret = can_register("/dev/can0", can);
|
/* Register the CAN driver at "/dev/can0" */
|
||||||
if (ret < 0)
|
|
||||||
{
|
|
||||||
candbg("ERROR: can_register failed: %d\n", ret);
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Now we are initialized */
|
ret = can_register("/dev/can0", can);
|
||||||
|
|
||||||
initialized = true;
|
if (ret < 0) {
|
||||||
}
|
candbg("ERROR: can_register failed: %d\n", ret);
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
return OK;
|
/* Now we are initialized */
|
||||||
|
|
||||||
|
initialized = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
|
@ -34,7 +34,7 @@
|
||||||
/**
|
/**
|
||||||
* @file px4fmu_init.c
|
* @file px4fmu_init.c
|
||||||
*
|
*
|
||||||
* PX4FMU-specific early startup code. This file implements the
|
* PX4FMU-specific early startup code. This file implements the
|
||||||
* nsh_archinitialize() function that is called early by nsh during startup.
|
* nsh_archinitialize() function that is called early by nsh during startup.
|
||||||
*
|
*
|
||||||
* Code here is run before the rcS script is invoked; it should start required
|
* Code here is run before the rcS script is invoked; it should start required
|
||||||
|
@ -115,11 +115,11 @@ extern int adc_devinit(void);
|
||||||
|
|
||||||
__EXPORT void stm32_boardinitialize(void)
|
__EXPORT void stm32_boardinitialize(void)
|
||||||
{
|
{
|
||||||
/* configure SPI interfaces */
|
/* configure SPI interfaces */
|
||||||
stm32_spiinitialize();
|
stm32_spiinitialize();
|
||||||
|
|
||||||
/* configure LEDs */
|
/* configure LEDs */
|
||||||
up_ledinit();
|
up_ledinit();
|
||||||
}
|
}
|
||||||
|
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
|
@ -139,171 +139,179 @@ static struct i2c_dev_s *i2c3;
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
__EXPORT int matherr(struct __exception *e) {
|
__EXPORT int matherr(struct __exception *e)
|
||||||
return 1;
|
{
|
||||||
|
return 1;
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
__EXPORT int matherr(struct exception *e) {
|
__EXPORT int matherr(struct exception *e)
|
||||||
return 1;
|
{
|
||||||
|
return 1;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
__EXPORT int nsh_archinitialize(void)
|
__EXPORT int nsh_archinitialize(void)
|
||||||
{
|
{
|
||||||
int result;
|
int result;
|
||||||
|
|
||||||
/* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */
|
/* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */
|
||||||
|
|
||||||
/* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */
|
/* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */
|
||||||
|
|
||||||
/* configure the high-resolution time/callout interface */
|
/* configure the high-resolution time/callout interface */
|
||||||
#ifdef CONFIG_HRT_TIMER
|
#ifdef CONFIG_HRT_TIMER
|
||||||
hrt_init();
|
hrt_init();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
/* configure CPU load estimation */
|
/* configure CPU load estimation */
|
||||||
#ifdef CONFIG_SCHED_INSTRUMENTATION
|
#ifdef CONFIG_SCHED_INSTRUMENTATION
|
||||||
cpuload_initialize_once();
|
cpuload_initialize_once();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
/* set up the serial DMA polling */
|
/* set up the serial DMA polling */
|
||||||
#ifdef SERIAL_HAVE_DMA
|
#ifdef SERIAL_HAVE_DMA
|
||||||
{
|
{
|
||||||
static struct hrt_call serial_dma_call;
|
static struct hrt_call serial_dma_call;
|
||||||
struct timespec ts;
|
struct timespec ts;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Poll at 1ms intervals for received bytes that have not triggered
|
* Poll at 1ms intervals for received bytes that have not triggered
|
||||||
* a DMA event.
|
* a DMA event.
|
||||||
*/
|
*/
|
||||||
ts.tv_sec = 0;
|
ts.tv_sec = 0;
|
||||||
ts.tv_nsec = 1000000;
|
ts.tv_nsec = 1000000;
|
||||||
|
|
||||||
hrt_call_every(&serial_dma_call,
|
hrt_call_every(&serial_dma_call,
|
||||||
ts_to_abstime(&ts),
|
ts_to_abstime(&ts),
|
||||||
ts_to_abstime(&ts),
|
ts_to_abstime(&ts),
|
||||||
(hrt_callout)stm32_serial_dma_poll,
|
(hrt_callout)stm32_serial_dma_poll,
|
||||||
NULL);
|
NULL);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
message("\r\n");
|
message("\r\n");
|
||||||
|
|
||||||
up_ledoff(LED_BLUE);
|
up_ledoff(LED_BLUE);
|
||||||
up_ledoff(LED_AMBER);
|
up_ledoff(LED_AMBER);
|
||||||
|
|
||||||
up_ledon(LED_BLUE);
|
up_ledon(LED_BLUE);
|
||||||
|
|
||||||
/* Configure user-space led driver */
|
/* Configure user-space led driver */
|
||||||
px4fmu_led_init();
|
px4fmu_led_init();
|
||||||
|
|
||||||
/* Configure SPI-based devices */
|
/* Configure SPI-based devices */
|
||||||
|
|
||||||
spi1 = up_spiinitialize(1);
|
spi1 = up_spiinitialize(1);
|
||||||
if (!spi1)
|
|
||||||
{
|
|
||||||
message("[boot] FAILED to initialize SPI port 1\r\n");
|
|
||||||
up_ledon(LED_AMBER);
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Default SPI1 to 1MHz and de-assert the known chip selects.
|
if (!spi1) {
|
||||||
SPI_SETFREQUENCY(spi1, 10000000);
|
message("[boot] FAILED to initialize SPI port 1\r\n");
|
||||||
SPI_SETBITS(spi1, 8);
|
up_ledon(LED_AMBER);
|
||||||
SPI_SETMODE(spi1, SPIDEV_MODE3);
|
return -ENODEV;
|
||||||
SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
|
}
|
||||||
SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false);
|
|
||||||
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
|
|
||||||
up_udelay(20);
|
|
||||||
|
|
||||||
message("[boot] Successfully initialized SPI port 1\r\n");
|
// Default SPI1 to 1MHz and de-assert the known chip selects.
|
||||||
|
SPI_SETFREQUENCY(spi1, 10000000);
|
||||||
|
SPI_SETBITS(spi1, 8);
|
||||||
|
SPI_SETMODE(spi1, SPIDEV_MODE3);
|
||||||
|
SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
|
||||||
|
SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false);
|
||||||
|
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
|
||||||
|
up_udelay(20);
|
||||||
|
|
||||||
/* initialize I2C2 bus */
|
message("[boot] Successfully initialized SPI port 1\r\n");
|
||||||
|
|
||||||
i2c2 = up_i2cinitialize(2);
|
/* initialize I2C2 bus */
|
||||||
if (!i2c2) {
|
|
||||||
message("[boot] FAILED to initialize I2C bus 2\n");
|
|
||||||
up_ledon(LED_AMBER);
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* set I2C2 speed */
|
i2c2 = up_i2cinitialize(2);
|
||||||
I2C_SETFREQUENCY(i2c2, 400000);
|
|
||||||
|
if (!i2c2) {
|
||||||
|
message("[boot] FAILED to initialize I2C bus 2\n");
|
||||||
|
up_ledon(LED_AMBER);
|
||||||
|
return -ENODEV;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* set I2C2 speed */
|
||||||
|
I2C_SETFREQUENCY(i2c2, 400000);
|
||||||
|
|
||||||
|
|
||||||
i2c3 = up_i2cinitialize(3);
|
i2c3 = up_i2cinitialize(3);
|
||||||
if (!i2c3) {
|
|
||||||
message("[boot] FAILED to initialize I2C bus 3\n");
|
|
||||||
up_ledon(LED_AMBER);
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* set I2C3 speed */
|
if (!i2c3) {
|
||||||
I2C_SETFREQUENCY(i2c3, 400000);
|
message("[boot] FAILED to initialize I2C bus 3\n");
|
||||||
|
up_ledon(LED_AMBER);
|
||||||
|
return -ENODEV;
|
||||||
|
}
|
||||||
|
|
||||||
/* try to attach, don't fail if device is not responding */
|
/* set I2C3 speed */
|
||||||
(void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS,
|
I2C_SETFREQUENCY(i2c3, 400000);
|
||||||
FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES,
|
|
||||||
FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES,
|
/* try to attach, don't fail if device is not responding */
|
||||||
FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1);
|
(void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS,
|
||||||
|
FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES,
|
||||||
|
FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES,
|
||||||
|
FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1);
|
||||||
|
|
||||||
#if defined(CONFIG_STM32_SPI3)
|
#if defined(CONFIG_STM32_SPI3)
|
||||||
/* Get the SPI port */
|
/* Get the SPI port */
|
||||||
|
|
||||||
message("[boot] Initializing SPI port 3\n");
|
message("[boot] Initializing SPI port 3\n");
|
||||||
spi3 = up_spiinitialize(3);
|
spi3 = up_spiinitialize(3);
|
||||||
if (!spi3)
|
|
||||||
{
|
|
||||||
message("[boot] FAILED to initialize SPI port 3\n");
|
|
||||||
up_ledon(LED_AMBER);
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
message("[boot] Successfully initialized SPI port 3\n");
|
|
||||||
|
|
||||||
/* Now bind the SPI interface to the MMCSD driver */
|
if (!spi3) {
|
||||||
result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
|
message("[boot] FAILED to initialize SPI port 3\n");
|
||||||
if (result != OK)
|
up_ledon(LED_AMBER);
|
||||||
{
|
return -ENODEV;
|
||||||
message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
|
}
|
||||||
up_ledon(LED_AMBER);
|
|
||||||
return -ENODEV;
|
message("[boot] Successfully initialized SPI port 3\n");
|
||||||
}
|
|
||||||
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
|
/* Now bind the SPI interface to the MMCSD driver */
|
||||||
|
result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
|
||||||
|
|
||||||
|
if (result != OK) {
|
||||||
|
message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
|
||||||
|
up_ledon(LED_AMBER);
|
||||||
|
return -ENODEV;
|
||||||
|
}
|
||||||
|
|
||||||
|
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
|
||||||
#endif /* SPI3 */
|
#endif /* SPI3 */
|
||||||
|
|
||||||
/* initialize I2C1 bus */
|
/* initialize I2C1 bus */
|
||||||
|
|
||||||
i2c1 = up_i2cinitialize(1);
|
i2c1 = up_i2cinitialize(1);
|
||||||
if (!i2c1) {
|
|
||||||
message("[boot] FAILED to initialize I2C bus 1\n");
|
|
||||||
up_ledon(LED_AMBER);
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* set I2C1 speed */
|
if (!i2c1) {
|
||||||
I2C_SETFREQUENCY(i2c1, 400000);
|
message("[boot] FAILED to initialize I2C bus 1\n");
|
||||||
|
up_ledon(LED_AMBER);
|
||||||
|
return -ENODEV;
|
||||||
|
}
|
||||||
|
|
||||||
/* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */
|
/* set I2C1 speed */
|
||||||
|
I2C_SETFREQUENCY(i2c1, 400000);
|
||||||
|
|
||||||
/* Get board information if available */
|
/* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */
|
||||||
|
|
||||||
/* Initialize the user GPIOs */
|
/* Get board information if available */
|
||||||
px4fmu_gpio_init();
|
|
||||||
|
/* Initialize the user GPIOs */
|
||||||
|
px4fmu_gpio_init();
|
||||||
|
|
||||||
#ifdef CONFIG_ADC
|
#ifdef CONFIG_ADC
|
||||||
int adc_state = adc_devinit();
|
int adc_state = adc_devinit();
|
||||||
if (adc_state != OK)
|
|
||||||
{
|
if (adc_state != OK) {
|
||||||
/* Try again */
|
/* Try again */
|
||||||
adc_state = adc_devinit();
|
adc_state = adc_devinit();
|
||||||
if (adc_state != OK)
|
|
||||||
{
|
if (adc_state != OK) {
|
||||||
/* Give up */
|
/* Give up */
|
||||||
message("[boot] FAILED adc_devinit: %d\n", adc_state);
|
message("[boot] FAILED adc_devinit: %d\n", adc_state);
|
||||||
return -ENODEV;
|
return -ENODEV;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
|
@ -95,21 +95,24 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
|
||||||
stm32_gpiowrite(GPIO_SPI_CS_MPU, selected);
|
stm32_gpiowrite(GPIO_SPI_CS_MPU, selected);
|
||||||
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected);
|
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case PX4_SPIDEV_ACCEL:
|
case PX4_SPIDEV_ACCEL:
|
||||||
/* Making sure the other peripherals are not selected */
|
/* Making sure the other peripherals are not selected */
|
||||||
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected);
|
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected);
|
||||||
stm32_gpiowrite(GPIO_SPI_CS_MPU, selected);
|
stm32_gpiowrite(GPIO_SPI_CS_MPU, selected);
|
||||||
stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected);
|
stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case PX4_SPIDEV_MPU:
|
case PX4_SPIDEV_MPU:
|
||||||
/* Making sure the other peripherals are not selected */
|
/* Making sure the other peripherals are not selected */
|
||||||
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected);
|
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected);
|
||||||
stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected);
|
stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected);
|
||||||
stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
|
stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -77,16 +77,16 @@
|
||||||
|
|
||||||
__EXPORT void stm32_usbinitialize(void)
|
__EXPORT void stm32_usbinitialize(void)
|
||||||
{
|
{
|
||||||
/* The OTG FS has an internal soft pull-up */
|
/* The OTG FS has an internal soft pull-up */
|
||||||
|
|
||||||
/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
|
/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
|
||||||
|
|
||||||
#ifdef CONFIG_STM32_OTGFS
|
#ifdef CONFIG_STM32_OTGFS
|
||||||
stm32_configgpio(GPIO_OTGFS_VBUS);
|
stm32_configgpio(GPIO_OTGFS_VBUS);
|
||||||
/* XXX We only support device mode
|
/* XXX We only support device mode
|
||||||
stm32_configgpio(GPIO_OTGFS_PWRON);
|
stm32_configgpio(GPIO_OTGFS_PWRON);
|
||||||
stm32_configgpio(GPIO_OTGFS_OVER);
|
stm32_configgpio(GPIO_OTGFS_OVER);
|
||||||
*/
|
*/
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -103,6 +103,6 @@ __EXPORT void stm32_usbinitialize(void)
|
||||||
|
|
||||||
__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
|
__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
|
||||||
{
|
{
|
||||||
ulldbg("resume: %d\n", resume);
|
ulldbg("resume: %d\n", resume);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -54,7 +54,7 @@
|
||||||
/**
|
/**
|
||||||
* Namespace encapsulating all device framework classes, functions and data.
|
* Namespace encapsulating all device framework classes, functions and data.
|
||||||
*/
|
*/
|
||||||
namespace device __EXPORT
|
namespace device __EXPORT
|
||||||
{
|
{
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -276,14 +276,14 @@ public:
|
||||||
*/
|
*/
|
||||||
virtual int poll(struct file *filp, struct pollfd *fds, bool setup);
|
virtual int poll(struct file *filp, struct pollfd *fds, bool setup);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Test whether the device is currently open.
|
* Test whether the device is currently open.
|
||||||
*
|
*
|
||||||
* This can be used to avoid tearing down a device that is still active.
|
* This can be used to avoid tearing down a device that is still active.
|
||||||
*
|
*
|
||||||
* @return True if the device is currently open.
|
* @return True if the device is currently open.
|
||||||
*/
|
*/
|
||||||
bool is_open() { return _open_count > 0; }
|
bool is_open() { return _open_count > 0; }
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
/**
|
/**
|
||||||
|
|
|
@ -121,7 +121,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len
|
||||||
unsigned tries = 0;
|
unsigned tries = 0;
|
||||||
|
|
||||||
do {
|
do {
|
||||||
// debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len);
|
// debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len);
|
||||||
|
|
||||||
msgs = 0;
|
msgs = 0;
|
||||||
|
|
||||||
|
@ -144,7 +144,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len
|
||||||
if (msgs == 0)
|
if (msgs == 0)
|
||||||
return -EINVAL;
|
return -EINVAL;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* I2C architecture means there is an unavoidable race here
|
* I2C architecture means there is an unavoidable race here
|
||||||
* if there are any devices on the bus with a different frequency
|
* if there are any devices on the bus with a different frequency
|
||||||
* preference. Really, this is pointless.
|
* preference. Really, this is pointless.
|
||||||
|
@ -154,7 +154,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len
|
||||||
|
|
||||||
if (ret == OK)
|
if (ret == OK)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
// reset the I2C bus to unwedge on error
|
// reset the I2C bus to unwedge on error
|
||||||
up_i2creset(_dev);
|
up_i2creset(_dev);
|
||||||
|
|
||||||
|
|
|
@ -42,7 +42,7 @@
|
||||||
|
|
||||||
#include <nuttx/i2c.h>
|
#include <nuttx/i2c.h>
|
||||||
|
|
||||||
namespace device __EXPORT
|
namespace device __EXPORT
|
||||||
{
|
{
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
|
@ -134,6 +134,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
|
||||||
/* do common setup */
|
/* do common setup */
|
||||||
if (!up_interrupt_context())
|
if (!up_interrupt_context())
|
||||||
SPI_LOCK(_dev, true);
|
SPI_LOCK(_dev, true);
|
||||||
|
|
||||||
SPI_SETFREQUENCY(_dev, _frequency);
|
SPI_SETFREQUENCY(_dev, _frequency);
|
||||||
SPI_SETMODE(_dev, _mode);
|
SPI_SETMODE(_dev, _mode);
|
||||||
SPI_SETBITS(_dev, 8);
|
SPI_SETBITS(_dev, 8);
|
||||||
|
@ -144,6 +145,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
|
||||||
|
|
||||||
/* and clean up */
|
/* and clean up */
|
||||||
SPI_SELECT(_dev, _device, false);
|
SPI_SELECT(_dev, _device, false);
|
||||||
|
|
||||||
if (!up_interrupt_context())
|
if (!up_interrupt_context())
|
||||||
SPI_LOCK(_dev, false);
|
SPI_LOCK(_dev, false);
|
||||||
|
|
||||||
|
|
|
@ -84,7 +84,7 @@ protected:
|
||||||
* If called from interrupt context, this interface does not lock
|
* If called from interrupt context, this interface does not lock
|
||||||
* the bus and may interfere with non-interrupt-context callers.
|
* the bus and may interfere with non-interrupt-context callers.
|
||||||
*
|
*
|
||||||
* Clients in a mixed interrupt/non-interrupt configuration must
|
* Clients in a mixed interrupt/non-interrupt configuration must
|
||||||
* ensure appropriate interlocking.
|
* ensure appropriate interlocking.
|
||||||
*
|
*
|
||||||
* At least one of send or recv must be non-null.
|
* At least one of send or recv must be non-null.
|
||||||
|
|
|
@ -83,7 +83,7 @@ ORB_DECLARE(sensor_accel);
|
||||||
/*
|
/*
|
||||||
* ioctl() definitions
|
* ioctl() definitions
|
||||||
*
|
*
|
||||||
* Accelerometer drivers also implement the generic sensor driver
|
* Accelerometer drivers also implement the generic sensor driver
|
||||||
* interfaces from drv_sensor.h
|
* interfaces from drv_sensor.h
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
|
@ -58,7 +58,7 @@ struct gyro_report {
|
||||||
float temperature; /**< temperature in degrees celcius */
|
float temperature; /**< temperature in degrees celcius */
|
||||||
float range_rad_s;
|
float range_rad_s;
|
||||||
float scaling;
|
float scaling;
|
||||||
|
|
||||||
int16_t x_raw;
|
int16_t x_raw;
|
||||||
int16_t y_raw;
|
int16_t y_raw;
|
||||||
int16_t z_raw;
|
int16_t z_raw;
|
||||||
|
|
|
@ -62,7 +62,7 @@ typedef uint64_t hrt_abstime;
|
||||||
* they are serialised with respect to each other, and must not
|
* they are serialised with respect to each other, and must not
|
||||||
* block.
|
* block.
|
||||||
*/
|
*/
|
||||||
typedef void (* hrt_callout)(void *arg);
|
typedef void (* hrt_callout)(void *arg);
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Callout record.
|
* Callout record.
|
||||||
|
@ -113,7 +113,7 @@ __EXPORT extern void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, h
|
||||||
__EXPORT extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg);
|
__EXPORT extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg);
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* If this returns true, the entry has been invoked and removed from the callout list,
|
* If this returns true, the entry has been invoked and removed from the callout list,
|
||||||
* or it has never been entered.
|
* or it has never been entered.
|
||||||
*
|
*
|
||||||
* Always returns false for repeating callouts.
|
* Always returns false for repeating callouts.
|
||||||
|
|
|
@ -59,7 +59,7 @@ struct mag_report {
|
||||||
float z;
|
float z;
|
||||||
float range_ga;
|
float range_ga;
|
||||||
float scaling;
|
float scaling;
|
||||||
|
|
||||||
int16_t x_raw;
|
int16_t x_raw;
|
||||||
int16_t y_raw;
|
int16_t y_raw;
|
||||||
int16_t z_raw;
|
int16_t z_raw;
|
||||||
|
|
|
@ -136,7 +136,7 @@ __EXPORT extern void up_pwm_servo_deinit(void);
|
||||||
*
|
*
|
||||||
* When disarmed, servos output no pulse.
|
* When disarmed, servos output no pulse.
|
||||||
*
|
*
|
||||||
* @bug This function should, but does not, guarantee that any pulse
|
* @bug This function should, but does not, guarantee that any pulse
|
||||||
* currently in progress is cleanly completed.
|
* currently in progress is cleanly completed.
|
||||||
*
|
*
|
||||||
* @param armed If true, outputs are armed; if false they
|
* @param armed If true, outputs are armed; if false they
|
||||||
|
|
|
@ -52,7 +52,7 @@
|
||||||
#define _SENSORIOC(_n) (_IOC(_SENSORIOCBASE, _n))
|
#define _SENSORIOC(_n) (_IOC(_SENSORIOCBASE, _n))
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE
|
* Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE
|
||||||
* constants
|
* constants
|
||||||
*/
|
*/
|
||||||
#define SENSORIOCSPOLLRATE _SENSORIOC(0)
|
#define SENSORIOCSPOLLRATE _SENSORIOC(0)
|
||||||
|
@ -68,8 +68,8 @@
|
||||||
#define SENSOR_POLLRATE_MAX 1000002 /**< poll at device maximum rate */
|
#define SENSOR_POLLRATE_MAX 1000002 /**< poll at device maximum rate */
|
||||||
#define SENSOR_POLLRATE_DEFAULT 1000003 /**< poll at driver normal rate */
|
#define SENSOR_POLLRATE_DEFAULT 1000003 /**< poll at driver normal rate */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Set the internal queue depth to (arg) entries, must be at least 1
|
* Set the internal queue depth to (arg) entries, must be at least 1
|
||||||
*
|
*
|
||||||
* This sets the upper bound on the number of readings that can be
|
* This sets the upper bound on the number of readings that can be
|
||||||
* read from the driver.
|
* read from the driver.
|
||||||
|
|
|
@ -34,8 +34,8 @@
|
||||||
/*
|
/*
|
||||||
* Driver for the PX4 audio alarm port, /dev/tone_alarm.
|
* Driver for the PX4 audio alarm port, /dev/tone_alarm.
|
||||||
*
|
*
|
||||||
* The tone_alarm driver supports a set of predefined "alarm"
|
* The tone_alarm driver supports a set of predefined "alarm"
|
||||||
* patterns and one user-supplied pattern. Patterns are ordered by
|
* patterns and one user-supplied pattern. Patterns are ordered by
|
||||||
* priority, with a higher-priority pattern interrupting any
|
* priority, with a higher-priority pattern interrupting any
|
||||||
* lower-priority pattern that might be playing.
|
* lower-priority pattern that might be playing.
|
||||||
*
|
*
|
||||||
|
|
|
@ -332,13 +332,16 @@ HMC5883::init()
|
||||||
/* allocate basic report buffers */
|
/* allocate basic report buffers */
|
||||||
_num_reports = 2;
|
_num_reports = 2;
|
||||||
_reports = new struct mag_report[_num_reports];
|
_reports = new struct mag_report[_num_reports];
|
||||||
|
|
||||||
if (_reports == nullptr)
|
if (_reports == nullptr)
|
||||||
goto out;
|
goto out;
|
||||||
|
|
||||||
_oldest_report = _next_report = 0;
|
_oldest_report = _next_report = 0;
|
||||||
|
|
||||||
/* get a publish handle on the mag topic */
|
/* get a publish handle on the mag topic */
|
||||||
memset(&_reports[0], 0, sizeof(_reports[0]));
|
memset(&_reports[0], 0, sizeof(_reports[0]));
|
||||||
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]);
|
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]);
|
||||||
|
|
||||||
if (_mag_topic < 0)
|
if (_mag_topic < 0)
|
||||||
debug("failed to create sensor_mag object");
|
debug("failed to create sensor_mag object");
|
||||||
|
|
||||||
|
@ -358,30 +361,37 @@ int HMC5883::set_range(unsigned range)
|
||||||
range_bits = 0x00;
|
range_bits = 0x00;
|
||||||
_range_scale = 1.0f / 1370.0f;
|
_range_scale = 1.0f / 1370.0f;
|
||||||
_range_ga = 0.88f;
|
_range_ga = 0.88f;
|
||||||
|
|
||||||
} else if (range <= 1) {
|
} else if (range <= 1) {
|
||||||
range_bits = 0x01;
|
range_bits = 0x01;
|
||||||
_range_scale = 1.0f / 1090.0f;
|
_range_scale = 1.0f / 1090.0f;
|
||||||
_range_ga = 1.3f;
|
_range_ga = 1.3f;
|
||||||
|
|
||||||
} else if (range <= 2) {
|
} else if (range <= 2) {
|
||||||
range_bits = 0x02;
|
range_bits = 0x02;
|
||||||
_range_scale = 1.0f / 820.0f;
|
_range_scale = 1.0f / 820.0f;
|
||||||
_range_ga = 1.9f;
|
_range_ga = 1.9f;
|
||||||
|
|
||||||
} else if (range <= 3) {
|
} else if (range <= 3) {
|
||||||
range_bits = 0x03;
|
range_bits = 0x03;
|
||||||
_range_scale = 1.0f / 660.0f;
|
_range_scale = 1.0f / 660.0f;
|
||||||
_range_ga = 2.5f;
|
_range_ga = 2.5f;
|
||||||
|
|
||||||
} else if (range <= 4) {
|
} else if (range <= 4) {
|
||||||
range_bits = 0x04;
|
range_bits = 0x04;
|
||||||
_range_scale = 1.0f / 440.0f;
|
_range_scale = 1.0f / 440.0f;
|
||||||
_range_ga = 4.0f;
|
_range_ga = 4.0f;
|
||||||
|
|
||||||
} else if (range <= 4.7f) {
|
} else if (range <= 4.7f) {
|
||||||
range_bits = 0x05;
|
range_bits = 0x05;
|
||||||
_range_scale = 1.0f / 390.0f;
|
_range_scale = 1.0f / 390.0f;
|
||||||
_range_ga = 4.7f;
|
_range_ga = 4.7f;
|
||||||
|
|
||||||
} else if (range <= 5.6f) {
|
} else if (range <= 5.6f) {
|
||||||
range_bits = 0x06;
|
range_bits = 0x06;
|
||||||
_range_scale = 1.0f / 330.0f;
|
_range_scale = 1.0f / 330.0f;
|
||||||
_range_ga = 5.6f;
|
_range_ga = 5.6f;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
range_bits = 0x07;
|
range_bits = 0x07;
|
||||||
_range_scale = 1.0f / 230.0f;
|
_range_scale = 1.0f / 230.0f;
|
||||||
|
@ -413,10 +423,12 @@ HMC5883::probe()
|
||||||
uint8_t data[3] = {0, 0, 0};
|
uint8_t data[3] = {0, 0, 0};
|
||||||
|
|
||||||
_retries = 10;
|
_retries = 10;
|
||||||
|
|
||||||
if (read_reg(ADDR_ID_A, data[0]) ||
|
if (read_reg(ADDR_ID_A, data[0]) ||
|
||||||
read_reg(ADDR_ID_B, data[1]) ||
|
read_reg(ADDR_ID_B, data[1]) ||
|
||||||
read_reg(ADDR_ID_C, data[2]))
|
read_reg(ADDR_ID_C, data[2]))
|
||||||
debug("read_reg fail");
|
debug("read_reg fail");
|
||||||
|
|
||||||
_retries = 1;
|
_retries = 1;
|
||||||
|
|
||||||
if ((data[0] != ID_A_WHO_AM_I) ||
|
if ((data[0] != ID_A_WHO_AM_I) ||
|
||||||
|
@ -552,6 +564,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
case SENSORIOCGPOLLRATE:
|
case SENSORIOCGPOLLRATE:
|
||||||
if (_measure_ticks == 0)
|
if (_measure_ticks == 0)
|
||||||
return SENSOR_POLLRATE_MANUAL;
|
return SENSOR_POLLRATE_MANUAL;
|
||||||
|
|
||||||
return (1000 / _measure_ticks);
|
return (1000 / _measure_ticks);
|
||||||
|
|
||||||
case SENSORIOCSQUEUEDEPTH: {
|
case SENSORIOCSQUEUEDEPTH: {
|
||||||
|
@ -666,7 +679,7 @@ HMC5883::cycle()
|
||||||
if (_measure_ticks > USEC2TICK(HMC5883_CONVERSION_INTERVAL)) {
|
if (_measure_ticks > USEC2TICK(HMC5883_CONVERSION_INTERVAL)) {
|
||||||
|
|
||||||
/* schedule a fresh cycle call when we are ready to measure again */
|
/* schedule a fresh cycle call when we are ready to measure again */
|
||||||
work_queue(HPWORK,
|
work_queue(HPWORK,
|
||||||
&_work,
|
&_work,
|
||||||
(worker_t)&HMC5883::cycle_trampoline,
|
(worker_t)&HMC5883::cycle_trampoline,
|
||||||
this,
|
this,
|
||||||
|
@ -684,7 +697,7 @@ HMC5883::cycle()
|
||||||
_collect_phase = true;
|
_collect_phase = true;
|
||||||
|
|
||||||
/* schedule a fresh cycle call when the measurement is done */
|
/* schedule a fresh cycle call when the measurement is done */
|
||||||
work_queue(HPWORK,
|
work_queue(HPWORK,
|
||||||
&_work,
|
&_work,
|
||||||
(worker_t)&HMC5883::cycle_trampoline,
|
(worker_t)&HMC5883::cycle_trampoline,
|
||||||
this,
|
this,
|
||||||
|
@ -850,7 +863,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
|
||||||
warnx("starting mag scale calibration");
|
warnx("starting mag scale calibration");
|
||||||
|
|
||||||
/* do a simple demand read */
|
/* do a simple demand read */
|
||||||
sz = read(filp, (char*)&report, sizeof(report));
|
sz = read(filp, (char *)&report, sizeof(report));
|
||||||
|
|
||||||
if (sz != sizeof(report)) {
|
if (sz != sizeof(report)) {
|
||||||
warn("immediate read failed");
|
warn("immediate read failed");
|
||||||
ret = 1;
|
ret = 1;
|
||||||
|
@ -920,6 +934,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
|
||||||
if (sz != sizeof(report)) {
|
if (sz != sizeof(report)) {
|
||||||
warn("periodic read failed");
|
warn("periodic read failed");
|
||||||
goto out;
|
goto out;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
avg_excited[0] += report.x;
|
avg_excited[0] += report.x;
|
||||||
avg_excited[1] += report.y;
|
avg_excited[1] += report.y;
|
||||||
|
@ -946,7 +961,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
|
||||||
scaling[2] = fabsf(1.08f / avg_excited[2]);
|
scaling[2] = fabsf(1.08f / avg_excited[2]);
|
||||||
|
|
||||||
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
|
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
|
||||||
|
|
||||||
/* set back to normal mode */
|
/* set back to normal mode */
|
||||||
/* Set to 1.1 Gauss */
|
/* Set to 1.1 Gauss */
|
||||||
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
|
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
|
||||||
|
@ -971,12 +986,15 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
|
||||||
|
|
||||||
ret = OK;
|
ret = OK;
|
||||||
|
|
||||||
out:
|
out:
|
||||||
if (ret == OK) {
|
|
||||||
warnx("mag scale calibration successfully finished.");
|
if (ret == OK) {
|
||||||
} else {
|
warnx("mag scale calibration successfully finished.");
|
||||||
warnx("mag scale calibration failed.");
|
|
||||||
}
|
} else {
|
||||||
|
warnx("mag scale calibration failed.");
|
||||||
|
}
|
||||||
|
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -986,16 +1004,22 @@ int HMC5883::set_excitement(unsigned enable)
|
||||||
/* arm the excitement strap */
|
/* arm the excitement strap */
|
||||||
uint8_t conf_reg;
|
uint8_t conf_reg;
|
||||||
ret = read_reg(ADDR_CONF_A, conf_reg);
|
ret = read_reg(ADDR_CONF_A, conf_reg);
|
||||||
|
|
||||||
if (OK != ret)
|
if (OK != ret)
|
||||||
perf_count(_comms_errors);
|
perf_count(_comms_errors);
|
||||||
|
|
||||||
if (((int)enable) < 0) {
|
if (((int)enable) < 0) {
|
||||||
conf_reg |= 0x01;
|
conf_reg |= 0x01;
|
||||||
|
|
||||||
} else if (enable > 0) {
|
} else if (enable > 0) {
|
||||||
conf_reg |= 0x02;
|
conf_reg |= 0x02;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
conf_reg &= ~0x03;
|
conf_reg &= ~0x03;
|
||||||
}
|
}
|
||||||
|
|
||||||
ret = write_reg(ADDR_CONF_A, conf_reg);
|
ret = write_reg(ADDR_CONF_A, conf_reg);
|
||||||
|
|
||||||
if (OK != ret)
|
if (OK != ret)
|
||||||
perf_count(_comms_errors);
|
perf_count(_comms_errors);
|
||||||
|
|
||||||
|
@ -1087,17 +1111,22 @@ start()
|
||||||
|
|
||||||
/* set the poll rate to default, starts automatic data collection */
|
/* set the poll rate to default, starts automatic data collection */
|
||||||
fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
goto fail;
|
goto fail;
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||||
goto fail;
|
goto fail;
|
||||||
|
|
||||||
exit(0);
|
exit(0);
|
||||||
|
|
||||||
fail:
|
fail:
|
||||||
|
|
||||||
if (g_dev != nullptr) {
|
if (g_dev != nullptr) {
|
||||||
delete g_dev;
|
delete g_dev;
|
||||||
g_dev = nullptr;
|
g_dev = nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
errx(1, "driver start failed");
|
errx(1, "driver start failed");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1114,11 +1143,13 @@ test()
|
||||||
int ret;
|
int ret;
|
||||||
|
|
||||||
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
|
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
|
||||||
|
|
||||||
/* do a simple demand read */
|
/* do a simple demand read */
|
||||||
sz = read(fd, &report, sizeof(report));
|
sz = read(fd, &report, sizeof(report));
|
||||||
|
|
||||||
if (sz != sizeof(report))
|
if (sz != sizeof(report))
|
||||||
err(1, "immediate read failed");
|
err(1, "immediate read failed");
|
||||||
|
|
||||||
|
@ -1167,7 +1198,7 @@ test()
|
||||||
* Basic idea:
|
* Basic idea:
|
||||||
*
|
*
|
||||||
* output = (ext field +- 1.1 Ga self-test) * scale factor
|
* output = (ext field +- 1.1 Ga self-test) * scale factor
|
||||||
*
|
*
|
||||||
* and consequently:
|
* and consequently:
|
||||||
*
|
*
|
||||||
* 1.1 Ga = (excited - normal) * scale factor
|
* 1.1 Ga = (excited - normal) * scale factor
|
||||||
|
@ -1207,6 +1238,7 @@ int calibrate()
|
||||||
int ret;
|
int ret;
|
||||||
|
|
||||||
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
|
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
|
||||||
|
|
||||||
|
@ -1218,6 +1250,7 @@ int calibrate()
|
||||||
|
|
||||||
if (ret == OK) {
|
if (ret == OK) {
|
||||||
errx(0, "PASS");
|
errx(0, "PASS");
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
errx(1, "FAIL");
|
errx(1, "FAIL");
|
||||||
}
|
}
|
||||||
|
@ -1230,10 +1263,13 @@ void
|
||||||
reset()
|
reset()
|
||||||
{
|
{
|
||||||
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
err(1, "failed ");
|
err(1, "failed ");
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
|
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
|
||||||
err(1, "driver reset failed");
|
err(1, "driver reset failed");
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||||
err(1, "driver poll restart failed");
|
err(1, "driver poll restart failed");
|
||||||
|
|
||||||
|
@ -1290,6 +1326,7 @@ hmc5883_main(int argc, char *argv[])
|
||||||
if (!strcmp(argv[1], "calibrate")) {
|
if (!strcmp(argv[1], "calibrate")) {
|
||||||
if (hmc5883::calibrate() == 0) {
|
if (hmc5883::calibrate() == 0) {
|
||||||
errx(0, "calibration successful");
|
errx(0, "calibration successful");
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
errx(1, "calibration failed");
|
errx(1, "calibration failed");
|
||||||
}
|
}
|
||||||
|
|
|
@ -317,6 +317,7 @@ L3GD20::init()
|
||||||
_num_reports = 2;
|
_num_reports = 2;
|
||||||
_oldest_report = _next_report = 0;
|
_oldest_report = _next_report = 0;
|
||||||
_reports = new struct gyro_report[_num_reports];
|
_reports = new struct gyro_report[_num_reports];
|
||||||
|
|
||||||
if (_reports == nullptr)
|
if (_reports == nullptr)
|
||||||
goto out;
|
goto out;
|
||||||
|
|
||||||
|
@ -330,7 +331,7 @@ L3GD20::init()
|
||||||
write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */
|
write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */
|
||||||
write_reg(ADDR_CTRL_REG4, REG4_BDU);
|
write_reg(ADDR_CTRL_REG4, REG4_BDU);
|
||||||
write_reg(ADDR_CTRL_REG5, 0);
|
write_reg(ADDR_CTRL_REG5, 0);
|
||||||
|
|
||||||
write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
|
write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
|
||||||
write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */
|
write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */
|
||||||
|
|
||||||
|
@ -451,6 +452,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
case SENSORIOCGPOLLRATE:
|
case SENSORIOCGPOLLRATE:
|
||||||
if (_call_interval == 0)
|
if (_call_interval == 0)
|
||||||
return SENSOR_POLLRATE_MANUAL;
|
return SENSOR_POLLRATE_MANUAL;
|
||||||
|
|
||||||
return 1000000 / _call_interval;
|
return 1000000 / _call_interval;
|
||||||
|
|
||||||
case SENSORIOCSQUEUEDEPTH: {
|
case SENSORIOCSQUEUEDEPTH: {
|
||||||
|
@ -478,7 +480,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
}
|
}
|
||||||
|
|
||||||
case SENSORIOCGQUEUEDEPTH:
|
case SENSORIOCGQUEUEDEPTH:
|
||||||
return _num_reports -1;
|
return _num_reports - 1;
|
||||||
|
|
||||||
case SENSORIOCRESET:
|
case SENSORIOCRESET:
|
||||||
/* XXX implement */
|
/* XXX implement */
|
||||||
|
@ -497,12 +499,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
|
|
||||||
case GYROIOCSSCALE:
|
case GYROIOCSSCALE:
|
||||||
/* copy scale in */
|
/* copy scale in */
|
||||||
memcpy(&_gyro_scale, (struct gyro_scale*) arg, sizeof(_gyro_scale));
|
memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale));
|
||||||
return OK;
|
return OK;
|
||||||
|
|
||||||
case GYROIOCGSCALE:
|
case GYROIOCGSCALE:
|
||||||
/* copy scale out */
|
/* copy scale out */
|
||||||
memcpy((struct gyro_scale*) arg, &_gyro_scale, sizeof(_gyro_scale));
|
memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale));
|
||||||
return OK;
|
return OK;
|
||||||
|
|
||||||
case GYROIOCSRANGE:
|
case GYROIOCSRANGE:
|
||||||
|
@ -562,12 +564,15 @@ L3GD20::set_range(unsigned max_dps)
|
||||||
if (max_dps <= 250) {
|
if (max_dps <= 250) {
|
||||||
_current_range = 250;
|
_current_range = 250;
|
||||||
bits |= RANGE_250DPS;
|
bits |= RANGE_250DPS;
|
||||||
|
|
||||||
} else if (max_dps <= 500) {
|
} else if (max_dps <= 500) {
|
||||||
_current_range = 500;
|
_current_range = 500;
|
||||||
bits |= RANGE_500DPS;
|
bits |= RANGE_500DPS;
|
||||||
|
|
||||||
} else if (max_dps <= 2000) {
|
} else if (max_dps <= 2000) {
|
||||||
_current_range = 2000;
|
_current_range = 2000;
|
||||||
bits |= RANGE_2000DPS;
|
bits |= RANGE_2000DPS;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
return -EINVAL;
|
return -EINVAL;
|
||||||
}
|
}
|
||||||
|
@ -590,15 +595,19 @@ L3GD20::set_samplerate(unsigned frequency)
|
||||||
if (frequency <= 95) {
|
if (frequency <= 95) {
|
||||||
_current_rate = 95;
|
_current_rate = 95;
|
||||||
bits |= RATE_95HZ_LP_25HZ;
|
bits |= RATE_95HZ_LP_25HZ;
|
||||||
|
|
||||||
} else if (frequency <= 190) {
|
} else if (frequency <= 190) {
|
||||||
_current_rate = 190;
|
_current_rate = 190;
|
||||||
bits |= RATE_190HZ_LP_25HZ;
|
bits |= RATE_190HZ_LP_25HZ;
|
||||||
|
|
||||||
} else if (frequency <= 380) {
|
} else if (frequency <= 380) {
|
||||||
_current_rate = 380;
|
_current_rate = 380;
|
||||||
bits |= RATE_380HZ_LP_30HZ;
|
bits |= RATE_380HZ_LP_30HZ;
|
||||||
|
|
||||||
} else if (frequency <= 760) {
|
} else if (frequency <= 760) {
|
||||||
_current_rate = 760;
|
_current_rate = 760;
|
||||||
bits |= RATE_760HZ_LP_30HZ;
|
bits |= RATE_760HZ_LP_30HZ;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
return -EINVAL;
|
return -EINVAL;
|
||||||
}
|
}
|
||||||
|
@ -746,17 +755,21 @@ start()
|
||||||
|
|
||||||
/* set the poll rate to default, starts automatic data collection */
|
/* set the poll rate to default, starts automatic data collection */
|
||||||
fd = open(GYRO_DEVICE_PATH, O_RDONLY);
|
fd = open(GYRO_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
goto fail;
|
goto fail;
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||||
goto fail;
|
goto fail;
|
||||||
|
|
||||||
exit(0);
|
exit(0);
|
||||||
fail:
|
fail:
|
||||||
|
|
||||||
if (g_dev != nullptr) {
|
if (g_dev != nullptr) {
|
||||||
delete g_dev;
|
delete g_dev;
|
||||||
g_dev = nullptr;
|
g_dev = nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
errx(1, "driver start failed");
|
errx(1, "driver start failed");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -774,15 +787,17 @@ test()
|
||||||
|
|
||||||
/* get the driver */
|
/* get the driver */
|
||||||
fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
|
fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd_gyro < 0)
|
if (fd_gyro < 0)
|
||||||
err(1, "%s open failed", GYRO_DEVICE_PATH);
|
err(1, "%s open failed", GYRO_DEVICE_PATH);
|
||||||
|
|
||||||
/* reset to manual polling */
|
/* reset to manual polling */
|
||||||
if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
|
if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
|
||||||
err(1, "reset to manual polling");
|
err(1, "reset to manual polling");
|
||||||
|
|
||||||
/* do a simple demand read */
|
/* do a simple demand read */
|
||||||
sz = read(fd_gyro, &g_report, sizeof(g_report));
|
sz = read(fd_gyro, &g_report, sizeof(g_report));
|
||||||
|
|
||||||
if (sz != sizeof(g_report))
|
if (sz != sizeof(g_report))
|
||||||
err(1, "immediate gyro read failed");
|
err(1, "immediate gyro read failed");
|
||||||
|
|
||||||
|
@ -793,7 +808,7 @@ test()
|
||||||
warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
|
warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
|
||||||
warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
|
warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
|
||||||
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
|
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
|
||||||
(int)((g_report.range_rad_s / M_PI_F) * 180.0f+0.5f));
|
(int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
|
||||||
|
|
||||||
/* XXX add poll-rate tests here too */
|
/* XXX add poll-rate tests here too */
|
||||||
|
|
||||||
|
@ -808,10 +823,13 @@ void
|
||||||
reset()
|
reset()
|
||||||
{
|
{
|
||||||
int fd = open(GYRO_DEVICE_PATH, O_RDONLY);
|
int fd = open(GYRO_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
err(1, "failed ");
|
err(1, "failed ");
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
|
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
|
||||||
err(1, "driver reset failed");
|
err(1, "driver reset failed");
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||||
err(1, "driver poll restart failed");
|
err(1, "driver poll restart failed");
|
||||||
|
|
||||||
|
|
|
@ -569,6 +569,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
case SENSORIOCGPOLLRATE:
|
case SENSORIOCGPOLLRATE:
|
||||||
if (_call_interval == 0)
|
if (_call_interval == 0)
|
||||||
return SENSOR_POLLRATE_MANUAL;
|
return SENSOR_POLLRATE_MANUAL;
|
||||||
|
|
||||||
return 1000000 / _call_interval;
|
return 1000000 / _call_interval;
|
||||||
|
|
||||||
case SENSORIOCSQUEUEDEPTH:
|
case SENSORIOCSQUEUEDEPTH:
|
||||||
|
@ -592,12 +593,12 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
|
|
||||||
case ACCELIOCSSCALE:
|
case ACCELIOCSSCALE:
|
||||||
/* copy scale in */
|
/* copy scale in */
|
||||||
memcpy(&_accel_scale, (struct accel_scale*) arg, sizeof(_accel_scale));
|
memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale));
|
||||||
return OK;
|
return OK;
|
||||||
|
|
||||||
case ACCELIOCGSCALE:
|
case ACCELIOCGSCALE:
|
||||||
/* copy scale out */
|
/* copy scale out */
|
||||||
memcpy((struct accel_scale*) arg, &_accel_scale, sizeof(_accel_scale));
|
memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
|
||||||
return OK;
|
return OK;
|
||||||
|
|
||||||
case ACCELIOCSRANGE:
|
case ACCELIOCSRANGE:
|
||||||
|
@ -639,12 +640,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
|
|
||||||
case GYROIOCSSCALE:
|
case GYROIOCSSCALE:
|
||||||
/* copy scale in */
|
/* copy scale in */
|
||||||
memcpy(&_gyro_scale, (struct gyro_scale*) arg, sizeof(_gyro_scale));
|
memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale));
|
||||||
return OK;
|
return OK;
|
||||||
|
|
||||||
case GYROIOCGSCALE:
|
case GYROIOCGSCALE:
|
||||||
/* copy scale out */
|
/* copy scale out */
|
||||||
memcpy((struct gyro_scale*) arg, &_gyro_scale, sizeof(_gyro_scale));
|
memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale));
|
||||||
return OK;
|
return OK;
|
||||||
|
|
||||||
case GYROIOCSRANGE:
|
case GYROIOCSRANGE:
|
||||||
|
@ -976,17 +977,21 @@ start()
|
||||||
|
|
||||||
/* set the poll rate to default, starts automatic data collection */
|
/* set the poll rate to default, starts automatic data collection */
|
||||||
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
goto fail;
|
goto fail;
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||||
goto fail;
|
goto fail;
|
||||||
|
|
||||||
exit(0);
|
exit(0);
|
||||||
fail:
|
fail:
|
||||||
|
|
||||||
if (g_dev != nullptr) {
|
if (g_dev != nullptr) {
|
||||||
delete g_dev;
|
delete g_dev;
|
||||||
g_dev = nullptr;
|
g_dev = nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
errx(1, "driver start failed");
|
errx(1, "driver start failed");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1006,21 +1011,24 @@ test()
|
||||||
|
|
||||||
/* get the driver */
|
/* get the driver */
|
||||||
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
|
err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
|
||||||
ACCEL_DEVICE_PATH);
|
ACCEL_DEVICE_PATH);
|
||||||
|
|
||||||
/* get the driver */
|
/* get the driver */
|
||||||
fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
|
fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd_gyro < 0)
|
if (fd_gyro < 0)
|
||||||
err(1, "%s open failed", GYRO_DEVICE_PATH);
|
err(1, "%s open failed", GYRO_DEVICE_PATH);
|
||||||
|
|
||||||
/* reset to manual polling */
|
/* reset to manual polling */
|
||||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
|
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
|
||||||
err(1, "reset to manual polling");
|
err(1, "reset to manual polling");
|
||||||
|
|
||||||
/* do a simple demand read */
|
/* do a simple demand read */
|
||||||
sz = read(fd, &a_report, sizeof(a_report));
|
sz = read(fd, &a_report, sizeof(a_report));
|
||||||
|
|
||||||
if (sz != sizeof(a_report))
|
if (sz != sizeof(a_report))
|
||||||
err(1, "immediate acc read failed");
|
err(1, "immediate acc read failed");
|
||||||
|
|
||||||
|
@ -1033,10 +1041,11 @@ test()
|
||||||
warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
|
warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
|
||||||
warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
|
warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
|
||||||
warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
|
warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
|
||||||
(double)(a_report.range_m_s2 / 9.81f));
|
(double)(a_report.range_m_s2 / 9.81f));
|
||||||
|
|
||||||
/* do a simple demand read */
|
/* do a simple demand read */
|
||||||
sz = read(fd_gyro, &g_report, sizeof(g_report));
|
sz = read(fd_gyro, &g_report, sizeof(g_report));
|
||||||
|
|
||||||
if (sz != sizeof(g_report))
|
if (sz != sizeof(g_report))
|
||||||
err(1, "immediate gyro read failed");
|
err(1, "immediate gyro read failed");
|
||||||
|
|
||||||
|
@ -1047,7 +1056,7 @@ test()
|
||||||
warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
|
warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
|
||||||
warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
|
warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
|
||||||
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
|
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
|
||||||
(int)((g_report.range_rad_s / M_PI_F) * 180.0f+0.5f));
|
(int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
|
||||||
|
|
||||||
warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature);
|
warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature);
|
||||||
warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
|
warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
|
||||||
|
@ -1066,10 +1075,13 @@ void
|
||||||
reset()
|
reset()
|
||||||
{
|
{
|
||||||
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
err(1, "failed ");
|
err(1, "failed ");
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
|
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
|
||||||
err(1, "driver reset failed");
|
err(1, "driver reset failed");
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||||
err(1, "driver poll restart failed");
|
err(1, "driver poll restart failed");
|
||||||
|
|
||||||
|
|
|
@ -303,6 +303,7 @@ MS5611::init()
|
||||||
/* allocate basic report buffers */
|
/* allocate basic report buffers */
|
||||||
_num_reports = 2;
|
_num_reports = 2;
|
||||||
_reports = new struct baro_report[_num_reports];
|
_reports = new struct baro_report[_num_reports];
|
||||||
|
|
||||||
if (_reports == nullptr)
|
if (_reports == nullptr)
|
||||||
goto out;
|
goto out;
|
||||||
|
|
||||||
|
@ -311,6 +312,7 @@ MS5611::init()
|
||||||
/* get a publish handle on the baro topic */
|
/* get a publish handle on the baro topic */
|
||||||
memset(&_reports[0], 0, sizeof(_reports[0]));
|
memset(&_reports[0], 0, sizeof(_reports[0]));
|
||||||
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]);
|
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]);
|
||||||
|
|
||||||
if (_baro_topic < 0)
|
if (_baro_topic < 0)
|
||||||
debug("failed to create sensor_baro object");
|
debug("failed to create sensor_baro object");
|
||||||
|
|
||||||
|
@ -323,9 +325,10 @@ int
|
||||||
MS5611::probe()
|
MS5611::probe()
|
||||||
{
|
{
|
||||||
_retries = 10;
|
_retries = 10;
|
||||||
if((OK == probe_address(MS5611_ADDRESS_1)) ||
|
|
||||||
(OK == probe_address(MS5611_ADDRESS_2))) {
|
if ((OK == probe_address(MS5611_ADDRESS_1)) ||
|
||||||
_retries = 1;
|
(OK == probe_address(MS5611_ADDRESS_2))) {
|
||||||
|
_retries = 1;
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -484,6 +487,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
case SENSORIOCGPOLLRATE:
|
case SENSORIOCGPOLLRATE:
|
||||||
if (_measure_ticks == 0)
|
if (_measure_ticks == 0)
|
||||||
return SENSOR_POLLRATE_MANUAL;
|
return SENSOR_POLLRATE_MANUAL;
|
||||||
|
|
||||||
return (1000 / _measure_ticks);
|
return (1000 / _measure_ticks);
|
||||||
|
|
||||||
case SENSORIOCSQUEUEDEPTH: {
|
case SENSORIOCSQUEUEDEPTH: {
|
||||||
|
@ -518,9 +522,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
return -EINVAL;
|
return -EINVAL;
|
||||||
|
|
||||||
case BAROIOCSMSLPRESSURE:
|
case BAROIOCSMSLPRESSURE:
|
||||||
|
|
||||||
/* range-check for sanity */
|
/* range-check for sanity */
|
||||||
if ((arg < 80000) || (arg > 120000))
|
if ((arg < 80000) || (arg > 120000))
|
||||||
return -EINVAL;
|
return -EINVAL;
|
||||||
|
|
||||||
_msl_pressure = arg;
|
_msl_pressure = arg;
|
||||||
return OK;
|
return OK;
|
||||||
|
|
||||||
|
@ -688,7 +694,7 @@ MS5611::collect()
|
||||||
int64_t SENS2 = 5 * f >> 2;
|
int64_t SENS2 = 5 * f >> 2;
|
||||||
|
|
||||||
if (_TEMP < -1500) {
|
if (_TEMP < -1500) {
|
||||||
int64_t f2 = POW2(_TEMP + 1500);
|
int64_t f2 = POW2(_TEMP + 1500);
|
||||||
OFF2 += 7 * f2;
|
OFF2 += 7 * f2;
|
||||||
SENS2 += 11 * f2 >> 1;
|
SENS2 += 11 * f2 >> 1;
|
||||||
}
|
}
|
||||||
|
@ -697,6 +703,7 @@ MS5611::collect()
|
||||||
_OFF -= OFF2;
|
_OFF -= OFF2;
|
||||||
_SENS -= SENS2;
|
_SENS -= SENS2;
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
||||||
/* pressure calculation, result in Pa */
|
/* pressure calculation, result in Pa */
|
||||||
|
@ -814,8 +821,8 @@ MS5611::read_prom()
|
||||||
uint16_t w;
|
uint16_t w;
|
||||||
} cvt;
|
} cvt;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Wait for PROM contents to be in the device (2.8 ms) in the case we are
|
* Wait for PROM contents to be in the device (2.8 ms) in the case we are
|
||||||
* called immediately after reset.
|
* called immediately after reset.
|
||||||
*/
|
*/
|
||||||
usleep(3000);
|
usleep(3000);
|
||||||
|
@ -941,17 +948,22 @@ start()
|
||||||
|
|
||||||
/* set the poll rate to default, starts automatic data collection */
|
/* set the poll rate to default, starts automatic data collection */
|
||||||
fd = open(BARO_DEVICE_PATH, O_RDONLY);
|
fd = open(BARO_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
goto fail;
|
goto fail;
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||||
goto fail;
|
goto fail;
|
||||||
|
|
||||||
exit(0);
|
exit(0);
|
||||||
|
|
||||||
fail:
|
fail:
|
||||||
|
|
||||||
if (g_dev != nullptr) {
|
if (g_dev != nullptr) {
|
||||||
delete g_dev;
|
delete g_dev;
|
||||||
g_dev = nullptr;
|
g_dev = nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
errx(1, "driver start failed");
|
errx(1, "driver start failed");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -968,11 +980,13 @@ test()
|
||||||
int ret;
|
int ret;
|
||||||
|
|
||||||
int fd = open(BARO_DEVICE_PATH, O_RDONLY);
|
int fd = open(BARO_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
|
err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
|
||||||
|
|
||||||
/* do a simple demand read */
|
/* do a simple demand read */
|
||||||
sz = read(fd, &report, sizeof(report));
|
sz = read(fd, &report, sizeof(report));
|
||||||
|
|
||||||
if (sz != sizeof(report))
|
if (sz != sizeof(report))
|
||||||
err(1, "immediate read failed");
|
err(1, "immediate read failed");
|
||||||
|
|
||||||
|
@ -1025,10 +1039,13 @@ void
|
||||||
reset()
|
reset()
|
||||||
{
|
{
|
||||||
int fd = open(BARO_DEVICE_PATH, O_RDONLY);
|
int fd = open(BARO_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
err(1, "failed ");
|
err(1, "failed ");
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
|
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
|
||||||
err(1, "driver reset failed");
|
err(1, "driver reset failed");
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||||
err(1, "driver poll restart failed");
|
err(1, "driver poll restart failed");
|
||||||
|
|
||||||
|
@ -1061,6 +1078,7 @@ calibrate(unsigned altitude)
|
||||||
float p1;
|
float p1;
|
||||||
|
|
||||||
int fd = open(BARO_DEVICE_PATH, O_RDONLY);
|
int fd = open(BARO_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
|
err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
|
||||||
|
|
||||||
|
@ -1070,6 +1088,7 @@ calibrate(unsigned altitude)
|
||||||
|
|
||||||
/* average a few measurements */
|
/* average a few measurements */
|
||||||
pressure = 0.0f;
|
pressure = 0.0f;
|
||||||
|
|
||||||
for (unsigned i = 0; i < 20; i++) {
|
for (unsigned i = 0; i < 20; i++) {
|
||||||
struct pollfd fds;
|
struct pollfd fds;
|
||||||
int ret;
|
int ret;
|
||||||
|
@ -1091,6 +1110,7 @@ calibrate(unsigned altitude)
|
||||||
|
|
||||||
pressure += report.pressure;
|
pressure += report.pressure;
|
||||||
}
|
}
|
||||||
|
|
||||||
pressure /= 20; /* average */
|
pressure /= 20; /* average */
|
||||||
pressure /= 10; /* scale from millibar to kPa */
|
pressure /= 10; /* scale from millibar to kPa */
|
||||||
|
|
||||||
|
@ -1108,8 +1128,10 @@ calibrate(unsigned altitude)
|
||||||
|
|
||||||
/* save as integer Pa */
|
/* save as integer Pa */
|
||||||
p1 *= 1000.0f;
|
p1 *= 1000.0f;
|
||||||
|
|
||||||
if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK)
|
if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK)
|
||||||
err(1, "BAROIOCSMSLPRESSURE");
|
err(1, "BAROIOCSMSLPRESSURE");
|
||||||
|
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1150,7 +1172,7 @@ ms5611_main(int argc, char *argv[])
|
||||||
errx(1, "missing altitude");
|
errx(1, "missing altitude");
|
||||||
|
|
||||||
long altitude = strtol(argv[2], nullptr, 10);
|
long altitude = strtol(argv[2], nullptr, 10);
|
||||||
|
|
||||||
ms5611::calibrate(altitude);
|
ms5611::calibrate(altitude);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -110,7 +110,7 @@ private:
|
||||||
bool _primary_pwm_device; ///< true if we are the default PWM output
|
bool _primary_pwm_device; ///< true if we are the default PWM output
|
||||||
|
|
||||||
volatile bool _switch_armed; ///< PX4IO switch armed state
|
volatile bool _switch_armed; ///< PX4IO switch armed state
|
||||||
// XXX how should this work?
|
// XXX how should this work?
|
||||||
|
|
||||||
bool _send_needed; ///< If true, we need to send a packet to IO
|
bool _send_needed; ///< If true, we need to send a packet to IO
|
||||||
|
|
||||||
|
@ -149,13 +149,13 @@ private:
|
||||||
* group/index during mixing.
|
* group/index during mixing.
|
||||||
*/
|
*/
|
||||||
static int control_callback(uintptr_t handle,
|
static int control_callback(uintptr_t handle,
|
||||||
uint8_t control_group,
|
uint8_t control_group,
|
||||||
uint8_t control_index,
|
uint8_t control_index,
|
||||||
float &input);
|
float &input);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
namespace
|
namespace
|
||||||
{
|
{
|
||||||
|
|
||||||
PX4IO *g_dev;
|
PX4IO *g_dev;
|
||||||
|
@ -190,6 +190,7 @@ PX4IO::~PX4IO()
|
||||||
|
|
||||||
/* spin waiting for the thread to stop */
|
/* spin waiting for the thread to stop */
|
||||||
unsigned i = 10;
|
unsigned i = 10;
|
||||||
|
|
||||||
do {
|
do {
|
||||||
/* wait 50ms - it should wake every 100ms or so worst-case */
|
/* wait 50ms - it should wake every 100ms or so worst-case */
|
||||||
usleep(50000);
|
usleep(50000);
|
||||||
|
@ -223,11 +224,13 @@ PX4IO::init()
|
||||||
|
|
||||||
/* do regular cdev init */
|
/* do regular cdev init */
|
||||||
ret = CDev::init();
|
ret = CDev::init();
|
||||||
|
|
||||||
if (ret != OK)
|
if (ret != OK)
|
||||||
return ret;
|
return ret;
|
||||||
|
|
||||||
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
|
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
|
||||||
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
|
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
|
||||||
|
|
||||||
if (ret == OK) {
|
if (ret == OK) {
|
||||||
log("default PWM output device");
|
log("default PWM output device");
|
||||||
_primary_pwm_device = true;
|
_primary_pwm_device = true;
|
||||||
|
@ -235,6 +238,7 @@ PX4IO::init()
|
||||||
|
|
||||||
/* start the IO interface task */
|
/* 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_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
|
||||||
|
|
||||||
if (_task < 0) {
|
if (_task < 0) {
|
||||||
debug("task start failed: %d", errno);
|
debug("task start failed: %d", errno);
|
||||||
return -errno;
|
return -errno;
|
||||||
|
@ -256,6 +260,7 @@ PX4IO::task_main()
|
||||||
|
|
||||||
/* open the serial port */
|
/* open the serial port */
|
||||||
_serial_fd = ::open("/dev/ttyS2", O_RDWR);
|
_serial_fd = ::open("/dev/ttyS2", O_RDWR);
|
||||||
|
|
||||||
if (_serial_fd < 0) {
|
if (_serial_fd < 0) {
|
||||||
debug("failed to open serial port for IO: %d", errno);
|
debug("failed to open serial port for IO: %d", errno);
|
||||||
_task = -1;
|
_task = -1;
|
||||||
|
@ -343,6 +348,7 @@ PX4IO::task_main()
|
||||||
_send_needed = true;
|
_send_needed = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (fds[2].revents & POLLIN) {
|
if (fds[2].revents & POLLIN) {
|
||||||
|
|
||||||
orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls);
|
orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls);
|
||||||
|
@ -364,9 +370,9 @@ PX4IO::task_main()
|
||||||
|
|
||||||
int
|
int
|
||||||
PX4IO::control_callback(uintptr_t handle,
|
PX4IO::control_callback(uintptr_t handle,
|
||||||
uint8_t control_group,
|
uint8_t control_group,
|
||||||
uint8_t control_index,
|
uint8_t control_index,
|
||||||
float &input)
|
float &input)
|
||||||
{
|
{
|
||||||
const actuator_controls_s *controls = (actuator_controls_s *)handle;
|
const actuator_controls_s *controls = (actuator_controls_s *)handle;
|
||||||
|
|
||||||
|
@ -458,13 +464,16 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
|
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)) {
|
if ((arg >= 900) && (arg <= 2100)) {
|
||||||
_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
|
_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
|
||||||
_send_needed = true;
|
_send_needed = true;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
ret = -EINVAL;
|
ret = -EINVAL;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
|
case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
|
||||||
|
@ -481,6 +490,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
||||||
delete _mixers;
|
delete _mixers;
|
||||||
_mixers = nullptr;
|
_mixers = nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MIXERIOCADDSIMPLE: {
|
case MIXERIOCADDSIMPLE: {
|
||||||
|
@ -519,6 +529,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
||||||
|
|
||||||
/* allocate a new mixer group and load it from the file */
|
/* allocate a new mixer group and load it from the file */
|
||||||
newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
|
newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
|
||||||
|
|
||||||
if (newmixers->load_from_file(path) != 0) {
|
if (newmixers->load_from_file(path) != 0) {
|
||||||
delete newmixers;
|
delete newmixers;
|
||||||
ret = -EINVAL;
|
ret = -EINVAL;
|
||||||
|
@ -528,6 +539,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
||||||
if (_mixers != nullptr) {
|
if (_mixers != nullptr) {
|
||||||
delete _mixers;
|
delete _mixers;
|
||||||
}
|
}
|
||||||
|
|
||||||
_mixers = newmixers;
|
_mixers = newmixers;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
@ -537,6 +549,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
||||||
/* not a recognised value */
|
/* not a recognised value */
|
||||||
ret = -ENOTTY;
|
ret = -ENOTTY;
|
||||||
}
|
}
|
||||||
|
|
||||||
unlock();
|
unlock();
|
||||||
|
|
||||||
return ret;
|
return ret;
|
||||||
|
@ -576,6 +589,7 @@ px4io_main(int argc, char *argv[])
|
||||||
if (argc > 2) {
|
if (argc > 2) {
|
||||||
fn[0] = argv[2];
|
fn[0] = argv[2];
|
||||||
fn[1] = nullptr;
|
fn[1] = nullptr;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
fn[0] = "/fs/microsd/px4io.bin";
|
fn[0] = "/fs/microsd/px4io.bin";
|
||||||
fn[1] = "/etc/px4io.bin";
|
fn[1] = "/etc/px4io.bin";
|
||||||
|
@ -589,18 +603,24 @@ px4io_main(int argc, char *argv[])
|
||||||
switch (ret) {
|
switch (ret) {
|
||||||
case OK:
|
case OK:
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case -ENOENT:
|
case -ENOENT:
|
||||||
errx(1, "PX4IO firmware file not found");
|
errx(1, "PX4IO firmware file not found");
|
||||||
|
|
||||||
case -EEXIST:
|
case -EEXIST:
|
||||||
case -EIO:
|
case -EIO:
|
||||||
errx(1, "error updating PX4IO - check that bootloader mode is enabled");
|
errx(1, "error updating PX4IO - check that bootloader mode is enabled");
|
||||||
|
|
||||||
case -EINVAL:
|
case -EINVAL:
|
||||||
errx(1, "verify failed - retry the update");
|
errx(1, "verify failed - retry the update");
|
||||||
|
|
||||||
case -ETIMEDOUT:
|
case -ETIMEDOUT:
|
||||||
errx(1, "timed out waiting for bootloader - power-cycle and try again");
|
errx(1, "timed out waiting for bootloader - power-cycle and try again");
|
||||||
|
|
||||||
default:
|
default:
|
||||||
errx(1, "unexpected error %d", ret);
|
errx(1, "unexpected error %d", ret);
|
||||||
}
|
}
|
||||||
|
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -67,6 +67,7 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||||
int ret;
|
int ret;
|
||||||
|
|
||||||
_io_fd = open("/dev/ttyS2", O_RDWR);
|
_io_fd = open("/dev/ttyS2", O_RDWR);
|
||||||
|
|
||||||
if (_io_fd < 0) {
|
if (_io_fd < 0) {
|
||||||
log("could not open interface");
|
log("could not open interface");
|
||||||
return -errno;
|
return -errno;
|
||||||
|
@ -74,6 +75,7 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||||
|
|
||||||
/* look for the bootloader */
|
/* look for the bootloader */
|
||||||
ret = sync();
|
ret = sync();
|
||||||
|
|
||||||
if (ret != OK) {
|
if (ret != OK) {
|
||||||
/* this is immediately fatal */
|
/* this is immediately fatal */
|
||||||
log("bootloader not responding");
|
log("bootloader not responding");
|
||||||
|
@ -87,17 +89,20 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||||
log("failed to open %s", filenames[i]);
|
log("failed to open %s", filenames[i]);
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
log("using firmware from %s", filenames[i]);
|
log("using firmware from %s", filenames[i]);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_fw_fd == -1)
|
if (_fw_fd == -1)
|
||||||
return -ENOENT;
|
return -ENOENT;
|
||||||
|
|
||||||
/* do the usual program thing - allow for failure */
|
/* do the usual program thing - allow for failure */
|
||||||
for (unsigned retries = 0; retries < 1; retries++) {
|
for (unsigned retries = 0; retries < 1; retries++) {
|
||||||
if (retries > 0) {
|
if (retries > 0) {
|
||||||
log("retrying update...");
|
log("retrying update...");
|
||||||
ret = sync();
|
ret = sync();
|
||||||
|
|
||||||
if (ret != OK) {
|
if (ret != OK) {
|
||||||
/* this is immediately fatal */
|
/* this is immediately fatal */
|
||||||
log("bootloader not responding");
|
log("bootloader not responding");
|
||||||
|
@ -106,25 +111,33 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||||
}
|
}
|
||||||
|
|
||||||
ret = erase();
|
ret = erase();
|
||||||
|
|
||||||
if (ret != OK) {
|
if (ret != OK) {
|
||||||
log("erase failed");
|
log("erase failed");
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
ret = program();
|
ret = program();
|
||||||
|
|
||||||
if (ret != OK) {
|
if (ret != OK) {
|
||||||
log("program failed");
|
log("program failed");
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
ret = verify();
|
ret = verify();
|
||||||
|
|
||||||
if (ret != OK) {
|
if (ret != OK) {
|
||||||
log("verify failed");
|
log("verify failed");
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
ret = reboot();
|
ret = reboot();
|
||||||
|
|
||||||
if (ret != OK) {
|
if (ret != OK) {
|
||||||
log("reboot failed");
|
log("reboot failed");
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
log("update complete");
|
log("update complete");
|
||||||
|
|
||||||
ret = OK;
|
ret = OK;
|
||||||
|
@ -145,6 +158,7 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
|
||||||
|
|
||||||
/* wait 100 ms for a character */
|
/* wait 100 ms for a character */
|
||||||
int ret = ::poll(&fds[0], 1, timeout);
|
int ret = ::poll(&fds[0], 1, timeout);
|
||||||
|
|
||||||
if (ret < 1) {
|
if (ret < 1) {
|
||||||
//log("poll timeout %d", ret);
|
//log("poll timeout %d", ret);
|
||||||
return -ETIMEDOUT;
|
return -ETIMEDOUT;
|
||||||
|
@ -160,9 +174,11 @@ PX4IO_Uploader::recv(uint8_t *p, unsigned count)
|
||||||
{
|
{
|
||||||
while (count--) {
|
while (count--) {
|
||||||
int ret = recv(*p++);
|
int ret = recv(*p++);
|
||||||
|
|
||||||
if (ret != OK)
|
if (ret != OK)
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -175,7 +191,7 @@ PX4IO_Uploader::drain()
|
||||||
do {
|
do {
|
||||||
ret = recv(c, 10);
|
ret = recv(c, 10);
|
||||||
//log("discard 0x%02x", c);
|
//log("discard 0x%02x", c);
|
||||||
} while(ret == OK);
|
} while (ret == OK);
|
||||||
}
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
|
@ -184,6 +200,7 @@ PX4IO_Uploader::send(uint8_t c)
|
||||||
//log("send 0x%02x", c);
|
//log("send 0x%02x", c);
|
||||||
if (write(_io_fd, &c, 1) != 1)
|
if (write(_io_fd, &c, 1) != 1)
|
||||||
return -errno;
|
return -errno;
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -192,9 +209,11 @@ PX4IO_Uploader::send(uint8_t *p, unsigned count)
|
||||||
{
|
{
|
||||||
while (count--) {
|
while (count--) {
|
||||||
int ret = send(*p++);
|
int ret = send(*p++);
|
||||||
|
|
||||||
if (ret != OK)
|
if (ret != OK)
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -205,15 +224,20 @@ PX4IO_Uploader::get_sync(unsigned timeout)
|
||||||
int ret;
|
int ret;
|
||||||
|
|
||||||
ret = recv(c[0], timeout);
|
ret = recv(c[0], timeout);
|
||||||
|
|
||||||
if (ret != OK)
|
if (ret != OK)
|
||||||
return ret;
|
return ret;
|
||||||
|
|
||||||
ret = recv(c[1], timeout);
|
ret = recv(c[1], timeout);
|
||||||
|
|
||||||
if (ret != OK)
|
if (ret != OK)
|
||||||
return ret;
|
return ret;
|
||||||
|
|
||||||
if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) {
|
if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) {
|
||||||
log("bad sync 0x%02x,0x%02x", c[0], c[1]);
|
log("bad sync 0x%02x,0x%02x", c[0], c[1]);
|
||||||
return -EIO;
|
return -EIO;
|
||||||
}
|
}
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -221,9 +245,11 @@ int
|
||||||
PX4IO_Uploader::sync()
|
PX4IO_Uploader::sync()
|
||||||
{
|
{
|
||||||
drain();
|
drain();
|
||||||
|
|
||||||
/* complete any pending program operation */
|
/* complete any pending program operation */
|
||||||
for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++)
|
for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++)
|
||||||
send(0);
|
send(0);
|
||||||
|
|
||||||
send(PROTO_GET_SYNC);
|
send(PROTO_GET_SYNC);
|
||||||
send(PROTO_EOC);
|
send(PROTO_EOC);
|
||||||
return get_sync();
|
return get_sync();
|
||||||
|
@ -239,8 +265,10 @@ PX4IO_Uploader::get_info(int param, uint32_t &val)
|
||||||
send(PROTO_EOC);
|
send(PROTO_EOC);
|
||||||
|
|
||||||
ret = recv((uint8_t *)&val, sizeof(val));
|
ret = recv((uint8_t *)&val, sizeof(val));
|
||||||
|
|
||||||
if (ret != OK)
|
if (ret != OK)
|
||||||
return ret;
|
return ret;
|
||||||
|
|
||||||
return get_sync();
|
return get_sync();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -267,10 +295,13 @@ PX4IO_Uploader::program()
|
||||||
/* get more bytes to program */
|
/* get more bytes to program */
|
||||||
//log(" %d", (int)lseek(_fw_fd, 0, SEEK_CUR));
|
//log(" %d", (int)lseek(_fw_fd, 0, SEEK_CUR));
|
||||||
count = read(_fw_fd, file_buf, sizeof(file_buf));
|
count = read(_fw_fd, file_buf, sizeof(file_buf));
|
||||||
|
|
||||||
if (count == 0)
|
if (count == 0)
|
||||||
return OK;
|
return OK;
|
||||||
|
|
||||||
if (count < 0)
|
if (count < 0)
|
||||||
return -errno;
|
return -errno;
|
||||||
|
|
||||||
ASSERT((count % 4) == 0);
|
ASSERT((count % 4) == 0);
|
||||||
|
|
||||||
send(PROTO_PROG_MULTI);
|
send(PROTO_PROG_MULTI);
|
||||||
|
@ -279,6 +310,7 @@ PX4IO_Uploader::program()
|
||||||
send(PROTO_EOC);
|
send(PROTO_EOC);
|
||||||
|
|
||||||
ret = get_sync(1000);
|
ret = get_sync(1000);
|
||||||
|
|
||||||
if (ret != OK)
|
if (ret != OK)
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
@ -297,6 +329,7 @@ PX4IO_Uploader::verify()
|
||||||
send(PROTO_CHIP_VERIFY);
|
send(PROTO_CHIP_VERIFY);
|
||||||
send(PROTO_EOC);
|
send(PROTO_EOC);
|
||||||
ret = get_sync();
|
ret = get_sync();
|
||||||
|
|
||||||
if (ret != OK)
|
if (ret != OK)
|
||||||
return ret;
|
return ret;
|
||||||
|
|
||||||
|
@ -304,19 +337,24 @@ PX4IO_Uploader::verify()
|
||||||
/* get more bytes to verify */
|
/* get more bytes to verify */
|
||||||
int base = (int)lseek(_fw_fd, 0, SEEK_CUR);
|
int base = (int)lseek(_fw_fd, 0, SEEK_CUR);
|
||||||
count = read(_fw_fd, file_buf, sizeof(file_buf));
|
count = read(_fw_fd, file_buf, sizeof(file_buf));
|
||||||
|
|
||||||
if (count == 0)
|
if (count == 0)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
if (count < 0)
|
if (count < 0)
|
||||||
return -errno;
|
return -errno;
|
||||||
|
|
||||||
ASSERT((count % 4) == 0);
|
ASSERT((count % 4) == 0);
|
||||||
|
|
||||||
send(PROTO_READ_MULTI);
|
send(PROTO_READ_MULTI);
|
||||||
send(count);
|
send(count);
|
||||||
send(PROTO_EOC);
|
send(PROTO_EOC);
|
||||||
|
|
||||||
for (ssize_t i = 0; i < count; i++) {
|
for (ssize_t i = 0; i < count; i++) {
|
||||||
uint8_t c;
|
uint8_t c;
|
||||||
|
|
||||||
ret = recv(c);
|
ret = recv(c);
|
||||||
|
|
||||||
if (ret != OK) {
|
if (ret != OK) {
|
||||||
log("%d: got %d waiting for bytes", base + i, ret);
|
log("%d: got %d waiting for bytes", base + i, ret);
|
||||||
return ret;
|
return ret;
|
||||||
|
@ -327,12 +365,15 @@ PX4IO_Uploader::verify()
|
||||||
return -EINVAL;
|
return -EINVAL;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
ret = get_sync();
|
ret = get_sync();
|
||||||
|
|
||||||
if (ret != OK) {
|
if (ret != OK) {
|
||||||
log("timeout waiting for post-verify sync");
|
log("timeout waiting for post-verify sync");
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -358,6 +399,7 @@ PX4IO_Uploader::compare(bool &identical)
|
||||||
send(PROTO_CHIP_VERIFY);
|
send(PROTO_CHIP_VERIFY);
|
||||||
send(PROTO_EOC);
|
send(PROTO_EOC);
|
||||||
ret = get_sync();
|
ret = get_sync();
|
||||||
|
|
||||||
if (ret != OK)
|
if (ret != OK)
|
||||||
return ret;
|
return ret;
|
||||||
|
|
||||||
|
@ -365,6 +407,7 @@ PX4IO_Uploader::compare(bool &identical)
|
||||||
send(sizeof(fw_vectors));
|
send(sizeof(fw_vectors));
|
||||||
send(PROTO_EOC);
|
send(PROTO_EOC);
|
||||||
ret = recv((uint8_t *)&fw_vectors[0], sizeof(fw_vectors));
|
ret = recv((uint8_t *)&fw_vectors[0], sizeof(fw_vectors));
|
||||||
|
|
||||||
if (ret != OK)
|
if (ret != OK)
|
||||||
return ret;
|
return ret;
|
||||||
|
|
||||||
|
|
|
@ -30,7 +30,7 @@
|
||||||
* POSSIBILITY OF SUCH DAMAGE.
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
*
|
*
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file drv_hrt.c
|
* @file drv_hrt.c
|
||||||
*
|
*
|
||||||
|
@ -41,7 +41,7 @@
|
||||||
* Note that really, this could use systick too, but that's
|
* Note that really, this could use systick too, but that's
|
||||||
* monopolised by NuttX and stealing it would just be awkward.
|
* monopolised by NuttX and stealing it would just be awkward.
|
||||||
*
|
*
|
||||||
* We don't use the NuttX STM32 driver per se; rather, we
|
* We don't use the NuttX STM32 driver per se; rather, we
|
||||||
* claim the timer and then drive it directly.
|
* claim the timer and then drive it directly.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
@ -264,10 +264,10 @@ static void hrt_latency_update(void);
|
||||||
|
|
||||||
/* callout list manipulation */
|
/* callout list manipulation */
|
||||||
static void hrt_call_internal(struct hrt_call *entry,
|
static void hrt_call_internal(struct hrt_call *entry,
|
||||||
hrt_abstime deadline,
|
hrt_abstime deadline,
|
||||||
hrt_abstime interval,
|
hrt_abstime interval,
|
||||||
hrt_callout callout,
|
hrt_callout callout,
|
||||||
void *arg);
|
void *arg);
|
||||||
static void hrt_call_enter(struct hrt_call *entry);
|
static void hrt_call_enter(struct hrt_call *entry);
|
||||||
static void hrt_call_reschedule(void);
|
static void hrt_call_reschedule(void);
|
||||||
static void hrt_call_invoke(void);
|
static void hrt_call_invoke(void);
|
||||||
|
@ -372,39 +372,39 @@ static void
|
||||||
hrt_tim_init(void)
|
hrt_tim_init(void)
|
||||||
{
|
{
|
||||||
/* clock/power on our timer */
|
/* clock/power on our timer */
|
||||||
modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT);
|
modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT);
|
||||||
|
|
||||||
/* claim our interrupt vector */
|
/* claim our interrupt vector */
|
||||||
irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr);
|
irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr);
|
||||||
|
|
||||||
/* disable and configure the timer */
|
/* disable and configure the timer */
|
||||||
rCR1 = 0;
|
rCR1 = 0;
|
||||||
rCR2 = 0;
|
rCR2 = 0;
|
||||||
rSMCR = 0;
|
rSMCR = 0;
|
||||||
rDIER = DIER_HRT | DIER_PPM;
|
rDIER = DIER_HRT | DIER_PPM;
|
||||||
rCCER = 0; /* unlock CCMR* registers */
|
rCCER = 0; /* unlock CCMR* registers */
|
||||||
rCCMR1 = CCMR1_PPM;
|
rCCMR1 = CCMR1_PPM;
|
||||||
rCCMR2 = CCMR2_PPM;
|
rCCMR2 = CCMR2_PPM;
|
||||||
rCCER = CCER_PPM;
|
rCCER = CCER_PPM;
|
||||||
rDCR = 0;
|
rDCR = 0;
|
||||||
|
|
||||||
/* configure the timer to free-run at 1MHz */
|
/* configure the timer to free-run at 1MHz */
|
||||||
rPSC = (HRT_TIMER_CLOCK / 1000000) - 1; /* this really only works for whole-MHz clocks */
|
rPSC = (HRT_TIMER_CLOCK / 1000000) - 1; /* this really only works for whole-MHz clocks */
|
||||||
|
|
||||||
/* run the full span of the counter */
|
/* run the full span of the counter */
|
||||||
rARR = 0xffff;
|
rARR = 0xffff;
|
||||||
|
|
||||||
/* set an initial capture a little ways off */
|
/* set an initial capture a little ways off */
|
||||||
rCCR_HRT = 1000;
|
rCCR_HRT = 1000;
|
||||||
|
|
||||||
/* generate an update event; reloads the counter, all registers */
|
/* generate an update event; reloads the counter, all registers */
|
||||||
rEGR = GTIM_EGR_UG;
|
rEGR = GTIM_EGR_UG;
|
||||||
|
|
||||||
/* enable the timer */
|
/* enable the timer */
|
||||||
rCR1 = GTIM_CR1_CEN;
|
rCR1 = GTIM_CR1_CEN;
|
||||||
|
|
||||||
/* enable interrupts */
|
/* enable interrupts */
|
||||||
up_enable_irq(HRT_TIMER_VECTOR);
|
up_enable_irq(HRT_TIMER_VECTOR);
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef CONFIG_HRT_PPM
|
#ifdef CONFIG_HRT_PPM
|
||||||
|
@ -412,7 +412,7 @@ hrt_tim_init(void)
|
||||||
* Handle the PPM decoder state machine.
|
* Handle the PPM decoder state machine.
|
||||||
*/
|
*/
|
||||||
static void
|
static void
|
||||||
hrt_ppm_decode(uint32_t status)
|
hrt_ppm_decode(uint32_t status)
|
||||||
{
|
{
|
||||||
uint16_t count = rCCR_PPM;
|
uint16_t count = rCCR_PPM;
|
||||||
uint16_t width;
|
uint16_t width;
|
||||||
|
@ -428,10 +428,11 @@ hrt_ppm_decode(uint32_t status)
|
||||||
ppm.last_edge = count;
|
ppm.last_edge = count;
|
||||||
|
|
||||||
ppm_edge_history[ppm_edge_next++] = width;
|
ppm_edge_history[ppm_edge_next++] = width;
|
||||||
|
|
||||||
if (ppm_edge_next >= 32)
|
if (ppm_edge_next >= 32)
|
||||||
ppm_edge_next = 0;
|
ppm_edge_next = 0;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* if this looks like a start pulse, then push the last set of values
|
* if this looks like a start pulse, then push the last set of values
|
||||||
* and reset the state machine
|
* and reset the state machine
|
||||||
*/
|
*/
|
||||||
|
@ -441,6 +442,7 @@ hrt_ppm_decode(uint32_t status)
|
||||||
if (ppm.next_channel > 4) {
|
if (ppm.next_channel > 4) {
|
||||||
for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++)
|
for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++)
|
||||||
ppm_buffer[i] = ppm_temp_buffer[i];
|
ppm_buffer[i] = ppm_temp_buffer[i];
|
||||||
|
|
||||||
ppm_decoded_channels = i;
|
ppm_decoded_channels = i;
|
||||||
ppm_last_valid_decode = hrt_absolute_time();
|
ppm_last_valid_decode = hrt_absolute_time();
|
||||||
|
|
||||||
|
@ -461,10 +463,11 @@ hrt_ppm_decode(uint32_t status)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
case ARM:
|
case ARM:
|
||||||
|
|
||||||
/* we expect a pulse giving us the first mark */
|
/* we expect a pulse giving us the first mark */
|
||||||
if (width > PPM_MAX_PULSE_WIDTH)
|
if (width > PPM_MAX_PULSE_WIDTH)
|
||||||
goto error; /* pulse was too long */
|
goto error; /* pulse was too long */
|
||||||
|
|
||||||
/* record the mark timing, expect an inactive edge */
|
/* record the mark timing, expect an inactive edge */
|
||||||
ppm.last_mark = count;
|
ppm.last_mark = count;
|
||||||
ppm.phase = INACTIVE;
|
ppm.phase = INACTIVE;
|
||||||
|
@ -481,6 +484,7 @@ hrt_ppm_decode(uint32_t status)
|
||||||
ppm.last_mark = count;
|
ppm.last_mark = count;
|
||||||
|
|
||||||
ppm_pulse_history[ppm_pulse_next++] = interval;
|
ppm_pulse_history[ppm_pulse_next++] = interval;
|
||||||
|
|
||||||
if (ppm_pulse_next >= 32)
|
if (ppm_pulse_next >= 32)
|
||||||
ppm_pulse_next = 0;
|
ppm_pulse_next = 0;
|
||||||
|
|
||||||
|
@ -493,7 +497,7 @@ hrt_ppm_decode(uint32_t status)
|
||||||
ppm_temp_buffer[ppm.next_channel++] = interval;
|
ppm_temp_buffer[ppm.next_channel++] = interval;
|
||||||
|
|
||||||
ppm.phase = INACTIVE;
|
ppm.phase = INACTIVE;
|
||||||
return;
|
return;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -526,9 +530,11 @@ hrt_tim_isr(int irq, void *context)
|
||||||
rSR = ~status;
|
rSR = ~status;
|
||||||
|
|
||||||
#ifdef CONFIG_HRT_PPM
|
#ifdef CONFIG_HRT_PPM
|
||||||
|
|
||||||
/* was this a PPM edge? */
|
/* was this a PPM edge? */
|
||||||
if (status & (SR_INT_PPM | SR_OVF_PPM))
|
if (status & (SR_INT_PPM | SR_OVF_PPM))
|
||||||
hrt_ppm_decode(status);
|
hrt_ppm_decode(status);
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
/* was this a timer tick? */
|
/* was this a timer tick? */
|
||||||
|
@ -640,7 +646,7 @@ hrt_init(void)
|
||||||
void
|
void
|
||||||
hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
|
hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
|
||||||
{
|
{
|
||||||
hrt_call_internal(entry,
|
hrt_call_internal(entry,
|
||||||
hrt_absolute_time() + delay,
|
hrt_absolute_time() + delay,
|
||||||
0,
|
0,
|
||||||
callout,
|
callout,
|
||||||
|
@ -730,9 +736,11 @@ hrt_call_enter(struct hrt_call *entry)
|
||||||
//lldbg("call enter at head, reschedule\n");
|
//lldbg("call enter at head, reschedule\n");
|
||||||
/* we changed the next deadline, reschedule the timer event */
|
/* we changed the next deadline, reschedule the timer event */
|
||||||
hrt_call_reschedule();
|
hrt_call_reschedule();
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
do {
|
do {
|
||||||
next = (struct hrt_call *)sq_next(&call->link);
|
next = (struct hrt_call *)sq_next(&call->link);
|
||||||
|
|
||||||
if ((next == NULL) || (entry->deadline < next->deadline)) {
|
if ((next == NULL) || (entry->deadline < next->deadline)) {
|
||||||
//lldbg("call enter after head\n");
|
//lldbg("call enter after head\n");
|
||||||
sq_addafter(&call->link, &entry->link, &callout_queue);
|
sq_addafter(&call->link, &entry->link, &callout_queue);
|
||||||
|
@ -755,8 +763,10 @@ hrt_call_invoke(void)
|
||||||
hrt_abstime now = hrt_absolute_time();
|
hrt_abstime now = hrt_absolute_time();
|
||||||
|
|
||||||
call = (struct hrt_call *)sq_peek(&callout_queue);
|
call = (struct hrt_call *)sq_peek(&callout_queue);
|
||||||
|
|
||||||
if (call == NULL)
|
if (call == NULL)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
if (call->deadline > now)
|
if (call->deadline > now)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
@ -764,7 +774,7 @@ hrt_call_invoke(void)
|
||||||
//lldbg("call pop\n");
|
//lldbg("call pop\n");
|
||||||
|
|
||||||
/* save the intended deadline for periodic calls */
|
/* save the intended deadline for periodic calls */
|
||||||
deadline = call->deadline;
|
deadline = call->deadline;
|
||||||
|
|
||||||
/* zero the deadline, as the call has occurred */
|
/* zero the deadline, as the call has occurred */
|
||||||
call->deadline = 0;
|
call->deadline = 0;
|
||||||
|
@ -804,7 +814,7 @@ hrt_call_reschedule()
|
||||||
* we want.
|
* we want.
|
||||||
*
|
*
|
||||||
* It is important for accurate timekeeping that the compare
|
* It is important for accurate timekeeping that the compare
|
||||||
* interrupt fires sufficiently often that the base_time update in
|
* interrupt fires sufficiently often that the base_time update in
|
||||||
* hrt_absolute_time runs at least once per timer period.
|
* hrt_absolute_time runs at least once per timer period.
|
||||||
*/
|
*/
|
||||||
if (next != NULL) {
|
if (next != NULL) {
|
||||||
|
@ -813,11 +823,13 @@ hrt_call_reschedule()
|
||||||
//lldbg("pre-expired\n");
|
//lldbg("pre-expired\n");
|
||||||
/* set a minimal deadline so that we call ASAP */
|
/* set a minimal deadline so that we call ASAP */
|
||||||
deadline = now + HRT_INTERVAL_MIN;
|
deadline = now + HRT_INTERVAL_MIN;
|
||||||
|
|
||||||
} else if (next->deadline < deadline) {
|
} else if (next->deadline < deadline) {
|
||||||
//lldbg("due soon\n");
|
//lldbg("due soon\n");
|
||||||
deadline = next->deadline;
|
deadline = next->deadline;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
//lldbg("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff));
|
//lldbg("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff));
|
||||||
|
|
||||||
/* set the new compare value and remember it for latency tracking */
|
/* set the new compare value and remember it for latency tracking */
|
||||||
|
@ -837,6 +849,7 @@ hrt_latency_update(void)
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* catch-all at the end */
|
/* catch-all at the end */
|
||||||
latency_counters[index]++;
|
latency_counters[index]++;
|
||||||
}
|
}
|
||||||
|
|
|
@ -197,26 +197,29 @@ pwm_channel_init(unsigned channel)
|
||||||
|
|
||||||
/* configure the channel */
|
/* configure the channel */
|
||||||
switch (pwm_channels[channel].timer_channel) {
|
switch (pwm_channels[channel].timer_channel) {
|
||||||
case 1:
|
case 1:
|
||||||
rCCMR1(timer) |= (6 << 4);
|
rCCMR1(timer) |= (6 << 4);
|
||||||
rCCR1(timer) = pwm_channels[channel].default_value;
|
rCCR1(timer) = pwm_channels[channel].default_value;
|
||||||
rCCER(timer) |= (1 << 0);
|
rCCER(timer) |= (1 << 0);
|
||||||
break;
|
break;
|
||||||
case 2:
|
|
||||||
rCCMR1(timer) |= (6 << 12);
|
case 2:
|
||||||
rCCR2(timer) = pwm_channels[channel].default_value;
|
rCCMR1(timer) |= (6 << 12);
|
||||||
rCCER(timer) |= (1 << 4);
|
rCCR2(timer) = pwm_channels[channel].default_value;
|
||||||
break;
|
rCCER(timer) |= (1 << 4);
|
||||||
case 3:
|
break;
|
||||||
rCCMR2(timer) |= (6 << 4);
|
|
||||||
rCCR3(timer) = pwm_channels[channel].default_value;
|
case 3:
|
||||||
rCCER(timer) |= (1 << 8);
|
rCCMR2(timer) |= (6 << 4);
|
||||||
break;
|
rCCR3(timer) = pwm_channels[channel].default_value;
|
||||||
case 4:
|
rCCER(timer) |= (1 << 8);
|
||||||
rCCMR2(timer) |= (6 << 12);
|
break;
|
||||||
rCCR4(timer) = pwm_channels[channel].default_value;
|
|
||||||
rCCER(timer) |= (1 << 12);
|
case 4:
|
||||||
break;
|
rCCMR2(timer) |= (6 << 12);
|
||||||
|
rCCR4(timer) = pwm_channels[channel].default_value;
|
||||||
|
rCCER(timer) |= (1 << 12);
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -238,22 +241,28 @@ up_pwm_servo_set(unsigned channel, servo_position_t value)
|
||||||
/* configure the channel */
|
/* configure the channel */
|
||||||
if (value > 0)
|
if (value > 0)
|
||||||
value--;
|
value--;
|
||||||
|
|
||||||
switch (pwm_channels[channel].timer_channel) {
|
switch (pwm_channels[channel].timer_channel) {
|
||||||
case 1:
|
case 1:
|
||||||
rCCR1(timer) = value;
|
rCCR1(timer) = value;
|
||||||
break;
|
break;
|
||||||
case 2:
|
|
||||||
rCCR2(timer) = value;
|
case 2:
|
||||||
break;
|
rCCR2(timer) = value;
|
||||||
case 3:
|
break;
|
||||||
rCCR3(timer) = value;
|
|
||||||
break;
|
case 3:
|
||||||
case 4:
|
rCCR3(timer) = value;
|
||||||
rCCR4(timer) = value;
|
break;
|
||||||
break;
|
|
||||||
default:
|
case 4:
|
||||||
return -1;
|
rCCR4(timer) = value;
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -275,19 +284,23 @@ up_pwm_servo_get(unsigned channel)
|
||||||
|
|
||||||
/* configure the channel */
|
/* configure the channel */
|
||||||
switch (pwm_channels[channel].timer_channel) {
|
switch (pwm_channels[channel].timer_channel) {
|
||||||
case 1:
|
case 1:
|
||||||
value = rCCR1(timer);
|
value = rCCR1(timer);
|
||||||
break;
|
break;
|
||||||
case 2:
|
|
||||||
value = rCCR2(timer);
|
case 2:
|
||||||
break;
|
value = rCCR2(timer);
|
||||||
case 3:
|
break;
|
||||||
value = rCCR3(timer);
|
|
||||||
break;
|
case 3:
|
||||||
case 4:
|
value = rCCR3(timer);
|
||||||
value = rCCR4(timer);
|
break;
|
||||||
break;
|
|
||||||
|
case 4:
|
||||||
|
value = rCCR4(timer);
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
return value;
|
return value;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -303,9 +316,10 @@ up_pwm_servo_init(uint32_t channel_mask)
|
||||||
/* now init channels */
|
/* now init channels */
|
||||||
for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
|
for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
|
||||||
/* don't do init for disabled channels; this leaves the pin configs alone */
|
/* don't do init for disabled channels; this leaves the pin configs alone */
|
||||||
if (((1<<i) & channel_mask) && (pwm_channels[i].gpio != 0))
|
if (((1 << i) & channel_mask) && (pwm_channels[i].gpio != 0))
|
||||||
pwm_channel_init(i);
|
pwm_channel_init(i);
|
||||||
}
|
}
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -326,17 +340,18 @@ up_pwm_servo_set_rate(unsigned rate)
|
||||||
if (pwm_timers[i].base != 0)
|
if (pwm_timers[i].base != 0)
|
||||||
pwm_timer_set_rate(i, rate);
|
pwm_timer_set_rate(i, rate);
|
||||||
}
|
}
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
up_pwm_servo_arm(bool armed)
|
up_pwm_servo_arm(bool armed)
|
||||||
{
|
{
|
||||||
/*
|
/*
|
||||||
* XXX this is inelgant and in particular will either jam outputs at whatever level
|
* XXX this is inelgant and in particular will either jam outputs at whatever level
|
||||||
* they happen to be at at the time the timers stop or generate runts.
|
* they happen to be at at the time the timers stop or generate runts.
|
||||||
* The right thing is almost certainly to kill auto-reload on the timers so that
|
* The right thing is almost certainly to kill auto-reload on the timers so that
|
||||||
* they just stop at the end of their count for disable, and to reset/restart them
|
* they just stop at the end of their count for disable, and to reset/restart them
|
||||||
* for enable.
|
* for enable.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
|
@ -34,8 +34,8 @@
|
||||||
/**
|
/**
|
||||||
* Driver for the PX4 audio alarm port, /dev/tone_alarm.
|
* Driver for the PX4 audio alarm port, /dev/tone_alarm.
|
||||||
*
|
*
|
||||||
* The tone_alarm driver supports a set of predefined "alarm"
|
* The tone_alarm driver supports a set of predefined "alarm"
|
||||||
* patterns and one user-supplied pattern. Patterns are ordered by
|
* patterns and one user-supplied pattern. Patterns are ordered by
|
||||||
* priority, with a higher-priority pattern interrupting any
|
* priority, with a higher-priority pattern interrupting any
|
||||||
* lower-priority pattern that might be playing.
|
* lower-priority pattern that might be playing.
|
||||||
*
|
*
|
||||||
|
@ -244,7 +244,8 @@ const tone_note ToneAlarm::_patterns[_max_pattern][_max_pattern_len] = {
|
||||||
{{TONE_NOTE_C7, 100}},
|
{{TONE_NOTE_C7, 100}},
|
||||||
{{TONE_NOTE_D7, 100}},
|
{{TONE_NOTE_D7, 100}},
|
||||||
{{TONE_NOTE_E7, 100}},
|
{{TONE_NOTE_E7, 100}},
|
||||||
{ //This is tetris ;)
|
{
|
||||||
|
//This is tetris ;)
|
||||||
{TONE_NOTE_C6, 40},
|
{TONE_NOTE_C6, 40},
|
||||||
{TONE_NOTE_G5, 20},
|
{TONE_NOTE_G5, 20},
|
||||||
{TONE_NOTE_G5S, 20},
|
{TONE_NOTE_G5S, 20},
|
||||||
|
@ -361,6 +362,7 @@ ToneAlarm::init()
|
||||||
int ret;
|
int ret;
|
||||||
|
|
||||||
ret = CDev::init();
|
ret = CDev::init();
|
||||||
|
|
||||||
if (ret != OK)
|
if (ret != OK)
|
||||||
return ret;
|
return ret;
|
||||||
|
|
||||||
|
@ -368,34 +370,34 @@ ToneAlarm::init()
|
||||||
stm32_configgpio(GPIO_TONE_ALARM);
|
stm32_configgpio(GPIO_TONE_ALARM);
|
||||||
|
|
||||||
/* clock/power on our timer */
|
/* clock/power on our timer */
|
||||||
modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE);
|
modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE);
|
||||||
|
|
||||||
/* initialise the timer */
|
/* initialise the timer */
|
||||||
rCR1 = 0;
|
rCR1 = 0;
|
||||||
rCR2 = 0;
|
rCR2 = 0;
|
||||||
rSMCR = 0;
|
rSMCR = 0;
|
||||||
rDIER = 0;
|
rDIER = 0;
|
||||||
rCCER &= TONE_CCER; /* unlock CCMR* registers */
|
rCCER &= TONE_CCER; /* unlock CCMR* registers */
|
||||||
rCCMR1 = TONE_CCMR1;
|
rCCMR1 = TONE_CCMR1;
|
||||||
rCCMR2 = TONE_CCMR2;
|
rCCMR2 = TONE_CCMR2;
|
||||||
rCCER = TONE_CCER;
|
rCCER = TONE_CCER;
|
||||||
rDCR = 0;
|
rDCR = 0;
|
||||||
|
|
||||||
/* toggle the CC output each time the count passes 1 */
|
/* toggle the CC output each time the count passes 1 */
|
||||||
TONE_rCCR = 1;
|
TONE_rCCR = 1;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Configure the timebase to free-run at half max frequency.
|
* Configure the timebase to free-run at half max frequency.
|
||||||
* XXX this should be more flexible in order to get a better
|
* XXX this should be more flexible in order to get a better
|
||||||
* frequency range, but for the F4 with the APB1 timers based
|
* frequency range, but for the F4 with the APB1 timers based
|
||||||
* at 42MHz, this gets us down to ~320Hz or so.
|
* at 42MHz, this gets us down to ~320Hz or so.
|
||||||
*/
|
*/
|
||||||
rPSC = 1;
|
rPSC = 1;
|
||||||
|
|
||||||
/* make sure the timer is running */
|
/* make sure the timer is running */
|
||||||
rCR1 = GTIM_CR1_CEN;
|
rCR1 = GTIM_CR1_CEN;
|
||||||
|
|
||||||
debug("ready");
|
debug("ready");
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -413,6 +415,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
|
||||||
switch (cmd) {
|
switch (cmd) {
|
||||||
case TONE_SET_ALARM:
|
case TONE_SET_ALARM:
|
||||||
debug("TONE_SET_ALARM %u", arg);
|
debug("TONE_SET_ALARM %u", arg);
|
||||||
|
|
||||||
if (new_pattern == 0) {
|
if (new_pattern == 0) {
|
||||||
/* cancel any current alarm */
|
/* cancel any current alarm */
|
||||||
_current_pattern = _pattern_none;
|
_current_pattern = _pattern_none;
|
||||||
|
@ -431,10 +434,13 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
|
||||||
|
|
||||||
/* and start playing it */
|
/* and start playing it */
|
||||||
next();
|
next();
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
/* current pattern is higher priority than the new pattern, ignore */
|
/* current pattern is higher priority than the new pattern, ignore */
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
result = -ENOTTY;
|
result = -ENOTTY;
|
||||||
break;
|
break;
|
||||||
|
@ -457,8 +463,10 @@ ToneAlarm::write(file *filp, const char *buffer, size_t len)
|
||||||
/* sanity-check the size of the write */
|
/* sanity-check the size of the write */
|
||||||
if (len > (_max_pattern_len * sizeof(tone_note)))
|
if (len > (_max_pattern_len * sizeof(tone_note)))
|
||||||
return -EFBIG;
|
return -EFBIG;
|
||||||
|
|
||||||
if ((len % sizeof(tone_note)) || (len == 0))
|
if ((len % sizeof(tone_note)) || (len == 0))
|
||||||
return -EIO;
|
return -EIO;
|
||||||
|
|
||||||
if (!check((tone_note *)buffer))
|
if (!check((tone_note *)buffer))
|
||||||
return -EIO;
|
return -EIO;
|
||||||
|
|
||||||
|
@ -479,6 +487,7 @@ ToneAlarm::write(file *filp, const char *buffer, size_t len)
|
||||||
debug("starting user pattern");
|
debug("starting user pattern");
|
||||||
next();
|
next();
|
||||||
}
|
}
|
||||||
|
|
||||||
irqrestore(flags);
|
irqrestore(flags);
|
||||||
|
|
||||||
return len;
|
return len;
|
||||||
|
@ -511,18 +520,22 @@ ToneAlarm::next(void)
|
||||||
/* find the note to play */
|
/* find the note to play */
|
||||||
if (_current_pattern == _pattern_user) {
|
if (_current_pattern == _pattern_user) {
|
||||||
np = &_user_pattern[_next_note];
|
np = &_user_pattern[_next_note];
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
np = &_patterns[_current_pattern - 1][_next_note];
|
np = &_patterns[_current_pattern - 1][_next_note];
|
||||||
}
|
}
|
||||||
|
|
||||||
/* work out which note is next */
|
/* work out which note is next */
|
||||||
_next_note++;
|
_next_note++;
|
||||||
|
|
||||||
if (_next_note >= _note_max) {
|
if (_next_note >= _note_max) {
|
||||||
/* hit the end of the pattern, stop */
|
/* hit the end of the pattern, stop */
|
||||||
_current_pattern = _pattern_none;
|
_current_pattern = _pattern_none;
|
||||||
|
|
||||||
} else if (np[1].duration == DURATION_END) {
|
} else if (np[1].duration == DURATION_END) {
|
||||||
/* hit the end of the pattern, stop */
|
/* hit the end of the pattern, stop */
|
||||||
_current_pattern = _pattern_none;
|
_current_pattern = _pattern_none;
|
||||||
|
|
||||||
} else if (np[1].duration == DURATION_REPEAT) {
|
} else if (np[1].duration == DURATION_REPEAT) {
|
||||||
/* next note is a repeat, rewind in preparation */
|
/* next note is a repeat, rewind in preparation */
|
||||||
_next_note = 0;
|
_next_note = 0;
|
||||||
|
@ -534,11 +547,11 @@ ToneAlarm::next(void)
|
||||||
/* set reload based on the pitch */
|
/* set reload based on the pitch */
|
||||||
rARR = _notes[np->pitch];
|
rARR = _notes[np->pitch];
|
||||||
|
|
||||||
/* force an update, reloads the counter and all registers */
|
/* force an update, reloads the counter and all registers */
|
||||||
rEGR = GTIM_EGR_UG;
|
rEGR = GTIM_EGR_UG;
|
||||||
|
|
||||||
/* enable the output */
|
/* enable the output */
|
||||||
rCCER |= TONE_CCER;
|
rCCER |= TONE_CCER;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* arrange a callback when the note/rest is done */
|
/* arrange a callback when the note/rest is done */
|
||||||
|
@ -554,6 +567,7 @@ ToneAlarm::check(tone_note *pattern)
|
||||||
if ((i == 0) &&
|
if ((i == 0) &&
|
||||||
((pattern[i].duration == DURATION_END) || (pattern[i].duration == DURATION_REPEAT)))
|
((pattern[i].duration == DURATION_END) || (pattern[i].duration == DURATION_REPEAT)))
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
if (pattern[i].duration == DURATION_END)
|
if (pattern[i].duration == DURATION_END)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
@ -561,6 +575,7 @@ ToneAlarm::check(tone_note *pattern)
|
||||||
if (pattern[i].pitch >= _note_max)
|
if (pattern[i].pitch >= _note_max)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -592,13 +607,16 @@ play_pattern(unsigned pattern)
|
||||||
int fd, ret;
|
int fd, ret;
|
||||||
|
|
||||||
fd = open("/dev/tone_alarm", 0);
|
fd = open("/dev/tone_alarm", 0);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
err(1, "/dev/tone_alarm");
|
err(1, "/dev/tone_alarm");
|
||||||
|
|
||||||
warnx("playing pattern %u", pattern);
|
warnx("playing pattern %u", pattern);
|
||||||
ret = ioctl(fd, TONE_SET_ALARM, pattern);
|
ret = ioctl(fd, TONE_SET_ALARM, pattern);
|
||||||
|
|
||||||
if (ret != 0)
|
if (ret != 0)
|
||||||
err(1, "TONE_SET_ALARM");
|
err(1, "TONE_SET_ALARM");
|
||||||
|
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -615,6 +633,7 @@ tone_alarm_main(int argc, char *argv[])
|
||||||
|
|
||||||
if (g_dev == nullptr)
|
if (g_dev == nullptr)
|
||||||
errx(1, "couldn't allocate the ToneAlarm driver");
|
errx(1, "couldn't allocate the ToneAlarm driver");
|
||||||
|
|
||||||
if (g_dev->init() != OK) {
|
if (g_dev->init() != OK) {
|
||||||
delete g_dev;
|
delete g_dev;
|
||||||
errx(1, "ToneAlarm init failed");
|
errx(1, "ToneAlarm init failed");
|
||||||
|
@ -623,8 +642,10 @@ tone_alarm_main(int argc, char *argv[])
|
||||||
|
|
||||||
if ((argc > 1) && !strcmp(argv[1], "start"))
|
if ((argc > 1) && !strcmp(argv[1], "start"))
|
||||||
play_pattern(1);
|
play_pattern(1);
|
||||||
|
|
||||||
if ((argc > 1) && !strcmp(argv[1], "stop"))
|
if ((argc > 1) && !strcmp(argv[1], "stop"))
|
||||||
play_pattern(0);
|
play_pattern(0);
|
||||||
|
|
||||||
if ((pattern = strtol(argv[1], nullptr, 10)) != 0)
|
if ((pattern = strtol(argv[1], nullptr, 10)) != 0)
|
||||||
play_pattern(pattern);
|
play_pattern(pattern);
|
||||||
|
|
||||||
|
|
|
@ -67,10 +67,12 @@ bl_update_main(int argc, char *argv[])
|
||||||
setopt();
|
setopt();
|
||||||
|
|
||||||
int fd = open(argv[1], O_RDONLY);
|
int fd = open(argv[1], O_RDONLY);
|
||||||
|
|
||||||
if (fd < 0)
|
if (fd < 0)
|
||||||
err(1, "open %s", argv[1]);
|
err(1, "open %s", argv[1]);
|
||||||
|
|
||||||
struct stat s;
|
struct stat s;
|
||||||
|
|
||||||
if (stat(argv[1], &s) < 0)
|
if (stat(argv[1], &s) < 0)
|
||||||
err(1, "stat %s", argv[1]);
|
err(1, "stat %s", argv[1]);
|
||||||
|
|
||||||
|
@ -79,14 +81,17 @@ bl_update_main(int argc, char *argv[])
|
||||||
errx(1, "%s: file too large", argv[1]);
|
errx(1, "%s: file too large", argv[1]);
|
||||||
|
|
||||||
uint8_t *buf = malloc(s.st_size);
|
uint8_t *buf = malloc(s.st_size);
|
||||||
|
|
||||||
if (buf == NULL)
|
if (buf == NULL)
|
||||||
errx(1, "failed to allocate %u bytes for firmware buffer", s.st_size);
|
errx(1, "failed to allocate %u bytes for firmware buffer", s.st_size);
|
||||||
|
|
||||||
if (read(fd, buf, s.st_size) != s.st_size)
|
if (read(fd, buf, s.st_size) != s.st_size)
|
||||||
err(1, "firmware read error");
|
err(1, "firmware read error");
|
||||||
|
|
||||||
close(fd);
|
close(fd);
|
||||||
|
|
||||||
uint32_t *hdr = (uint32_t *)buf;
|
uint32_t *hdr = (uint32_t *)buf;
|
||||||
|
|
||||||
if ((hdr[0] < 0x20000000) || /* stack not below RAM */
|
if ((hdr[0] < 0x20000000) || /* stack not below RAM */
|
||||||
(hdr[0] > (0x20000000 + (128 * 1024))) || /* stack not above RAM */
|
(hdr[0] > (0x20000000 + (128 * 1024))) || /* stack not above RAM */
|
||||||
(hdr[1] < 0x08000000) || /* entrypoint not below flash */
|
(hdr[1] < 0x08000000) || /* entrypoint not below flash */
|
||||||
|
@ -123,6 +128,7 @@ bl_update_main(int argc, char *argv[])
|
||||||
/* wait for the operation to complete */
|
/* wait for the operation to complete */
|
||||||
while (*sr & 0x1000) {
|
while (*sr & 0x1000) {
|
||||||
}
|
}
|
||||||
|
|
||||||
if (*sr & 0xf2) {
|
if (*sr & 0xf2) {
|
||||||
warnx("WARNING: erase error 0x%02x", *sr);
|
warnx("WARNING: erase error 0x%02x", *sr);
|
||||||
goto flash_end;
|
goto flash_end;
|
||||||
|
@ -148,6 +154,7 @@ bl_update_main(int argc, char *argv[])
|
||||||
/* wait for the operation to complete */
|
/* wait for the operation to complete */
|
||||||
while (*sr & 0x1000) {
|
while (*sr & 0x1000) {
|
||||||
}
|
}
|
||||||
|
|
||||||
if (*sr & 0xf2) {
|
if (*sr & 0xf2) {
|
||||||
warnx("WARNING: program error 0x%02x", *sr);
|
warnx("WARNING: program error 0x%02x", *sr);
|
||||||
goto flash_end;
|
goto flash_end;
|
||||||
|
@ -203,6 +210,7 @@ setopt(void)
|
||||||
|
|
||||||
if ((*optcr & opt_mask) == opt_bits)
|
if ((*optcr & opt_mask) == opt_bits)
|
||||||
errx(0, "option bits set");
|
errx(0, "option bits set");
|
||||||
|
|
||||||
errx(1, "option bits setting failed; readback 0x%04x", *optcr);
|
errx(1, "option bits setting failed; readback 0x%04x", *optcr);
|
||||||
|
|
||||||
}
|
}
|
|
@ -135,20 +135,19 @@
|
||||||
* cast between pointers to struct mtd_dev_s and struct at24c_dev_s.
|
* cast between pointers to struct mtd_dev_s and struct at24c_dev_s.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
struct at24c_dev_s
|
struct at24c_dev_s {
|
||||||
{
|
struct mtd_dev_s mtd; /* MTD interface */
|
||||||
struct mtd_dev_s mtd; /* MTD interface */
|
FAR struct i2c_dev_s *dev; /* Saved I2C interface instance */
|
||||||
FAR struct i2c_dev_s *dev; /* Saved I2C interface instance */
|
uint8_t addr; /* I2C address */
|
||||||
uint8_t addr; /* I2C address */
|
uint16_t pagesize; /* 32, 63 */
|
||||||
uint16_t pagesize; /* 32, 63 */
|
uint16_t npages; /* 128, 256, 512, 1024 */
|
||||||
uint16_t npages; /* 128, 256, 512, 1024 */
|
|
||||||
|
|
||||||
perf_counter_t perf_reads;
|
perf_counter_t perf_reads;
|
||||||
perf_counter_t perf_writes;
|
perf_counter_t perf_writes;
|
||||||
perf_counter_t perf_resets;
|
perf_counter_t perf_resets;
|
||||||
perf_counter_t perf_read_retries;
|
perf_counter_t perf_read_retries;
|
||||||
perf_counter_t perf_read_errors;
|
perf_counter_t perf_read_errors;
|
||||||
perf_counter_t perf_write_errors;
|
perf_counter_t perf_write_errors;
|
||||||
};
|
};
|
||||||
|
|
||||||
/************************************************************************************
|
/************************************************************************************
|
||||||
|
@ -159,9 +158,9 @@ struct at24c_dev_s
|
||||||
|
|
||||||
static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks);
|
static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks);
|
||||||
static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
|
static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
|
||||||
size_t nblocks, FAR uint8_t *buf);
|
size_t nblocks, FAR uint8_t *buf);
|
||||||
static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock,
|
static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock,
|
||||||
size_t nblocks, FAR const uint8_t *buf);
|
size_t nblocks, FAR const uint8_t *buf);
|
||||||
static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg);
|
static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg);
|
||||||
|
|
||||||
/************************************************************************************
|
/************************************************************************************
|
||||||
|
@ -180,35 +179,32 @@ static struct at24c_dev_s g_at24c;
|
||||||
|
|
||||||
static int at24c_eraseall(FAR struct at24c_dev_s *priv)
|
static int at24c_eraseall(FAR struct at24c_dev_s *priv)
|
||||||
{
|
{
|
||||||
int startblock = 0;
|
int startblock = 0;
|
||||||
uint8_t buf[AT24XX_PAGESIZE + 2];
|
uint8_t buf[AT24XX_PAGESIZE + 2];
|
||||||
|
|
||||||
struct i2c_msg_s msgv[1] =
|
struct i2c_msg_s msgv[1] = {
|
||||||
{
|
{
|
||||||
{
|
.addr = priv->addr,
|
||||||
.addr = priv->addr,
|
.flags = 0,
|
||||||
.flags = 0,
|
.buffer = &buf[0],
|
||||||
.buffer = &buf[0],
|
.length = sizeof(buf),
|
||||||
.length = sizeof(buf),
|
}
|
||||||
}
|
};
|
||||||
};
|
|
||||||
|
|
||||||
memset(&buf[2],0xff,priv->pagesize);
|
memset(&buf[2], 0xff, priv->pagesize);
|
||||||
|
|
||||||
for (startblock = 0; startblock < priv->npages; startblock++)
|
for (startblock = 0; startblock < priv->npages; startblock++) {
|
||||||
{
|
uint16_t offset = startblock * priv->pagesize;
|
||||||
uint16_t offset = startblock * priv->pagesize;
|
buf[1] = offset & 0xff;
|
||||||
buf[1] = offset & 0xff;
|
buf[0] = (offset >> 8) & 0xff;
|
||||||
buf[0] = (offset >> 8) & 0xff;
|
|
||||||
|
|
||||||
while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0)
|
while (I2C_TRANSFER(priv->dev, &msgv[0], 1) < 0) {
|
||||||
{
|
fvdbg("erase stall\n");
|
||||||
fvdbg("erase stall\n");
|
usleep(10000);
|
||||||
usleep(10000);
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
/************************************************************************************
|
/************************************************************************************
|
||||||
|
@ -217,9 +213,9 @@ static int at24c_eraseall(FAR struct at24c_dev_s *priv)
|
||||||
|
|
||||||
static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks)
|
static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks)
|
||||||
{
|
{
|
||||||
/* EEprom need not erase */
|
/* EEprom need not erase */
|
||||||
|
|
||||||
return (int)nblocks;
|
return (int)nblocks;
|
||||||
}
|
}
|
||||||
|
|
||||||
/************************************************************************************
|
/************************************************************************************
|
||||||
|
@ -227,90 +223,86 @@ static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nbloc
|
||||||
************************************************************************************/
|
************************************************************************************/
|
||||||
|
|
||||||
static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
|
static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
|
||||||
size_t nblocks, FAR uint8_t *buffer)
|
size_t nblocks, FAR uint8_t *buffer)
|
||||||
{
|
{
|
||||||
FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
|
FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
|
||||||
size_t blocksleft;
|
size_t blocksleft;
|
||||||
uint8_t addr[2];
|
uint8_t addr[2];
|
||||||
int ret;
|
int ret;
|
||||||
|
|
||||||
struct i2c_msg_s msgv[2] =
|
struct i2c_msg_s msgv[2] = {
|
||||||
{
|
{
|
||||||
{
|
.addr = priv->addr,
|
||||||
.addr = priv->addr,
|
.flags = 0,
|
||||||
.flags = 0,
|
.buffer = &addr[0],
|
||||||
.buffer = &addr[0],
|
.length = sizeof(addr),
|
||||||
.length = sizeof(addr),
|
},
|
||||||
},
|
{
|
||||||
{
|
.addr = priv->addr,
|
||||||
.addr = priv->addr,
|
.flags = I2C_M_READ,
|
||||||
.flags = I2C_M_READ,
|
.buffer = 0,
|
||||||
.buffer = 0,
|
.length = priv->pagesize,
|
||||||
.length = priv->pagesize,
|
}
|
||||||
}
|
};
|
||||||
};
|
|
||||||
|
|
||||||
#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
|
#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
|
||||||
startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
|
startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
|
||||||
nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
|
nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
|
||||||
#endif
|
#endif
|
||||||
blocksleft = nblocks;
|
blocksleft = nblocks;
|
||||||
|
|
||||||
fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
|
fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
|
||||||
|
|
||||||
if (startblock >= priv->npages)
|
if (startblock >= priv->npages) {
|
||||||
{
|
return 0;
|
||||||
return 0;
|
}
|
||||||
}
|
|
||||||
|
|
||||||
if (startblock + nblocks > priv->npages)
|
if (startblock + nblocks > priv->npages) {
|
||||||
{
|
nblocks = priv->npages - startblock;
|
||||||
nblocks = priv->npages - startblock;
|
}
|
||||||
}
|
|
||||||
|
|
||||||
while (blocksleft-- > 0)
|
while (blocksleft-- > 0) {
|
||||||
{
|
uint16_t offset = startblock * priv->pagesize;
|
||||||
uint16_t offset = startblock * priv->pagesize;
|
unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS;
|
||||||
unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS;
|
|
||||||
|
|
||||||
addr[1] = offset & 0xff;
|
addr[1] = offset & 0xff;
|
||||||
addr[0] = (offset >> 8) & 0xff;
|
addr[0] = (offset >> 8) & 0xff;
|
||||||
msgv[1].buffer = buffer;
|
msgv[1].buffer = buffer;
|
||||||
|
|
||||||
for (;;)
|
for (;;) {
|
||||||
{
|
|
||||||
|
|
||||||
perf_begin(priv->perf_reads);
|
perf_begin(priv->perf_reads);
|
||||||
ret = I2C_TRANSFER(priv->dev, &msgv[0], 2);
|
ret = I2C_TRANSFER(priv->dev, &msgv[0], 2);
|
||||||
perf_end(priv->perf_reads);
|
perf_end(priv->perf_reads);
|
||||||
if (ret >= 0)
|
|
||||||
break;
|
|
||||||
|
|
||||||
fvdbg("read stall");
|
if (ret >= 0)
|
||||||
usleep(1000);
|
break;
|
||||||
|
|
||||||
/* We should normally only be here on the first read after
|
fvdbg("read stall");
|
||||||
* a write.
|
usleep(1000);
|
||||||
*
|
|
||||||
* XXX maybe do special first-read handling with optional
|
|
||||||
* bus reset as well?
|
|
||||||
*/
|
|
||||||
perf_count(priv->perf_read_retries);
|
|
||||||
if (--tries == 0)
|
|
||||||
{
|
|
||||||
perf_count(priv->perf_read_errors);
|
|
||||||
return ERROR;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
startblock++;
|
/* We should normally only be here on the first read after
|
||||||
buffer += priv->pagesize;
|
* a write.
|
||||||
}
|
*
|
||||||
|
* XXX maybe do special first-read handling with optional
|
||||||
|
* bus reset as well?
|
||||||
|
*/
|
||||||
|
perf_count(priv->perf_read_retries);
|
||||||
|
|
||||||
|
if (--tries == 0) {
|
||||||
|
perf_count(priv->perf_read_errors);
|
||||||
|
return ERROR;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
startblock++;
|
||||||
|
buffer += priv->pagesize;
|
||||||
|
}
|
||||||
|
|
||||||
#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
|
#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
|
||||||
return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
|
return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
|
||||||
#else
|
#else
|
||||||
return nblocks;
|
return nblocks;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -322,81 +314,75 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
|
||||||
************************************************************************************/
|
************************************************************************************/
|
||||||
|
|
||||||
static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks,
|
static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks,
|
||||||
FAR const uint8_t *buffer)
|
FAR const uint8_t *buffer)
|
||||||
{
|
{
|
||||||
FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
|
FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
|
||||||
size_t blocksleft;
|
size_t blocksleft;
|
||||||
uint8_t buf[AT24XX_PAGESIZE+2];
|
uint8_t buf[AT24XX_PAGESIZE + 2];
|
||||||
int ret;
|
int ret;
|
||||||
|
|
||||||
struct i2c_msg_s msgv[1] =
|
struct i2c_msg_s msgv[1] = {
|
||||||
{
|
{
|
||||||
{
|
.addr = priv->addr,
|
||||||
.addr = priv->addr,
|
.flags = 0,
|
||||||
.flags = 0,
|
.buffer = &buf[0],
|
||||||
.buffer = &buf[0],
|
.length = sizeof(buf),
|
||||||
.length = sizeof(buf),
|
}
|
||||||
}
|
};
|
||||||
};
|
|
||||||
|
|
||||||
#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
|
#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
|
||||||
startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
|
startblock *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
|
||||||
nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
|
nblocks *= (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
|
||||||
#endif
|
#endif
|
||||||
blocksleft = nblocks;
|
blocksleft = nblocks;
|
||||||
|
|
||||||
if (startblock >= priv->npages)
|
if (startblock >= priv->npages) {
|
||||||
{
|
return 0;
|
||||||
return 0;
|
}
|
||||||
}
|
|
||||||
|
|
||||||
if (startblock + nblocks > priv->npages)
|
if (startblock + nblocks > priv->npages) {
|
||||||
{
|
nblocks = priv->npages - startblock;
|
||||||
nblocks = priv->npages - startblock;
|
}
|
||||||
}
|
|
||||||
|
|
||||||
fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
|
fvdbg("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
|
||||||
|
|
||||||
while (blocksleft-- > 0)
|
while (blocksleft-- > 0) {
|
||||||
{
|
uint16_t offset = startblock * priv->pagesize;
|
||||||
uint16_t offset = startblock * priv->pagesize;
|
unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS;
|
||||||
unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS;
|
|
||||||
|
|
||||||
buf[1] = offset & 0xff;
|
buf[1] = offset & 0xff;
|
||||||
buf[0] = (offset >> 8) & 0xff;
|
buf[0] = (offset >> 8) & 0xff;
|
||||||
memcpy(&buf[2], buffer, priv->pagesize);
|
memcpy(&buf[2], buffer, priv->pagesize);
|
||||||
|
|
||||||
for (;;)
|
for (;;) {
|
||||||
{
|
|
||||||
|
|
||||||
perf_begin(priv->perf_writes);
|
perf_begin(priv->perf_writes);
|
||||||
ret = I2C_TRANSFER(priv->dev, &msgv[0], 1);
|
ret = I2C_TRANSFER(priv->dev, &msgv[0], 1);
|
||||||
perf_end(priv->perf_writes);
|
perf_end(priv->perf_writes);
|
||||||
|
|
||||||
if (ret >= 0)
|
if (ret >= 0)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
fvdbg("write stall");
|
fvdbg("write stall");
|
||||||
usleep(1000);
|
usleep(1000);
|
||||||
|
|
||||||
/* We expect to see a number of retries per write cycle as we
|
/* We expect to see a number of retries per write cycle as we
|
||||||
* poll for write completion.
|
* poll for write completion.
|
||||||
*/
|
*/
|
||||||
if (--tries == 0)
|
if (--tries == 0) {
|
||||||
{
|
perf_count(priv->perf_write_errors);
|
||||||
perf_count(priv->perf_write_errors);
|
return ERROR;
|
||||||
return ERROR;
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
startblock++;
|
startblock++;
|
||||||
buffer += priv->pagesize;
|
buffer += priv->pagesize;
|
||||||
}
|
}
|
||||||
|
|
||||||
#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
|
#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
|
||||||
return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
|
return nblocks / (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE);
|
||||||
#else
|
#else
|
||||||
return nblocks;
|
return nblocks;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -406,67 +392,65 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t
|
||||||
|
|
||||||
static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg)
|
static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg)
|
||||||
{
|
{
|
||||||
FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
|
FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev;
|
||||||
int ret = -EINVAL; /* Assume good command with bad parameters */
|
int ret = -EINVAL; /* Assume good command with bad parameters */
|
||||||
|
|
||||||
fvdbg("cmd: %d \n", cmd);
|
fvdbg("cmd: %d \n", cmd);
|
||||||
|
|
||||||
switch (cmd)
|
switch (cmd) {
|
||||||
{
|
case MTDIOC_GEOMETRY: {
|
||||||
case MTDIOC_GEOMETRY:
|
FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg);
|
||||||
{
|
|
||||||
FAR struct mtd_geometry_s *geo = (FAR struct mtd_geometry_s *)((uintptr_t)arg);
|
if (geo) {
|
||||||
if (geo)
|
/* Populate the geometry structure with information need to know
|
||||||
{
|
* the capacity and how to access the device.
|
||||||
/* Populate the geometry structure with information need to know
|
*
|
||||||
* the capacity and how to access the device.
|
* NOTE: that the device is treated as though it where just an array
|
||||||
*
|
* of fixed size blocks. That is most likely not true, but the client
|
||||||
* NOTE: that the device is treated as though it where just an array
|
* will expect the device logic to do whatever is necessary to make it
|
||||||
* of fixed size blocks. That is most likely not true, but the client
|
* appear so.
|
||||||
* will expect the device logic to do whatever is necessary to make it
|
*
|
||||||
* appear so.
|
* blocksize:
|
||||||
*
|
* May be user defined. The block size for the at24XX devices may be
|
||||||
* blocksize:
|
* larger than the page size in order to better support file systems.
|
||||||
* May be user defined. The block size for the at24XX devices may be
|
* The read and write functions translate BLOCKS to pages for the
|
||||||
* larger than the page size in order to better support file systems.
|
* small flash devices
|
||||||
* The read and write functions translate BLOCKS to pages for the
|
* erasesize:
|
||||||
* small flash devices
|
* It has to be at least as big as the blocksize, bigger serves no
|
||||||
* erasesize:
|
* purpose.
|
||||||
* It has to be at least as big as the blocksize, bigger serves no
|
* neraseblocks
|
||||||
* purpose.
|
* Note that the device size is in kilobits and must be scaled by
|
||||||
* neraseblocks
|
* 1024 / 8
|
||||||
* Note that the device size is in kilobits and must be scaled by
|
*/
|
||||||
* 1024 / 8
|
|
||||||
*/
|
|
||||||
|
|
||||||
#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
|
#if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE
|
||||||
geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE;
|
geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE;
|
||||||
geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE;
|
geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE;
|
||||||
geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE;
|
geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE;
|
||||||
#else
|
#else
|
||||||
geo->blocksize = priv->pagesize;
|
geo->blocksize = priv->pagesize;
|
||||||
geo->erasesize = priv->pagesize;
|
geo->erasesize = priv->pagesize;
|
||||||
geo->neraseblocks = priv->npages;
|
geo->neraseblocks = priv->npages;
|
||||||
#endif
|
#endif
|
||||||
ret = OK;
|
ret = OK;
|
||||||
|
|
||||||
fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n",
|
fvdbg("blocksize: %d erasesize: %d neraseblocks: %d\n",
|
||||||
geo->blocksize, geo->erasesize, geo->neraseblocks);
|
geo->blocksize, geo->erasesize, geo->neraseblocks);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MTDIOC_BULKERASE:
|
case MTDIOC_BULKERASE:
|
||||||
ret=at24c_eraseall(priv);
|
ret = at24c_eraseall(priv);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MTDIOC_XIPBASE:
|
case MTDIOC_XIPBASE:
|
||||||
default:
|
default:
|
||||||
ret = -ENOTTY; /* Bad command */
|
ret = -ENOTTY; /* Bad command */
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
/************************************************************************************
|
/************************************************************************************
|
||||||
|
@ -483,46 +467,45 @@ static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg)
|
||||||
*
|
*
|
||||||
************************************************************************************/
|
************************************************************************************/
|
||||||
|
|
||||||
FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev)
|
FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) {
|
||||||
{
|
FAR struct at24c_dev_s *priv;
|
||||||
FAR struct at24c_dev_s *priv;
|
|
||||||
|
|
||||||
fvdbg("dev: %p\n", dev);
|
fvdbg("dev: %p\n", dev);
|
||||||
|
|
||||||
/* Allocate a state structure (we allocate the structure instead of using
|
/* Allocate a state structure (we allocate the structure instead of using
|
||||||
* a fixed, static allocation so that we can handle multiple FLASH devices.
|
* a fixed, static allocation so that we can handle multiple FLASH devices.
|
||||||
* The current implementation would handle only one FLASH part per I2C
|
* The current implementation would handle only one FLASH part per I2C
|
||||||
* device (only because of the SPIDEV_FLASH definition) and so would have
|
* device (only because of the SPIDEV_FLASH definition) and so would have
|
||||||
* to be extended to handle multiple FLASH parts on the same I2C bus.
|
* to be extended to handle multiple FLASH parts on the same I2C bus.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
priv = &g_at24c;
|
priv = &g_at24c;
|
||||||
if (priv)
|
|
||||||
{
|
|
||||||
/* Initialize the allocated structure */
|
|
||||||
|
|
||||||
priv->addr = CONFIG_AT24XX_ADDR;
|
|
||||||
priv->pagesize = AT24XX_PAGESIZE;
|
|
||||||
priv->npages = AT24XX_NPAGES;
|
|
||||||
|
|
||||||
priv->mtd.erase = at24c_erase;
|
if (priv) {
|
||||||
priv->mtd.bread = at24c_bread;
|
/* Initialize the allocated structure */
|
||||||
priv->mtd.bwrite = at24c_bwrite;
|
|
||||||
priv->mtd.ioctl = at24c_ioctl;
|
|
||||||
priv->dev = dev;
|
|
||||||
|
|
||||||
priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read");
|
priv->addr = CONFIG_AT24XX_ADDR;
|
||||||
priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write");
|
priv->pagesize = AT24XX_PAGESIZE;
|
||||||
priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset");
|
priv->npages = AT24XX_NPAGES;
|
||||||
priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries");
|
|
||||||
priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors");
|
|
||||||
priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors");
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Return the implementation-specific state structure as the MTD device */
|
priv->mtd.erase = at24c_erase;
|
||||||
|
priv->mtd.bread = at24c_bread;
|
||||||
|
priv->mtd.bwrite = at24c_bwrite;
|
||||||
|
priv->mtd.ioctl = at24c_ioctl;
|
||||||
|
priv->dev = dev;
|
||||||
|
|
||||||
fvdbg("Return %p\n", priv);
|
priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read");
|
||||||
return (FAR struct mtd_dev_s *)priv;
|
priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write");
|
||||||
|
priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset");
|
||||||
|
priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries");
|
||||||
|
priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors");
|
||||||
|
priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors");
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Return the implementation-specific state structure as the MTD device */
|
||||||
|
|
||||||
|
fvdbg("Return %p\n", priv);
|
||||||
|
return (FAR struct mtd_dev_s *)priv;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -530,5 +513,5 @@ FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev)
|
||||||
*/
|
*/
|
||||||
int at24c_nuke(void)
|
int at24c_nuke(void)
|
||||||
{
|
{
|
||||||
return at24c_eraseall(&g_at24c);
|
return at24c_eraseall(&g_at24c);
|
||||||
}
|
}
|
||||||
|
|
|
@ -143,13 +143,13 @@ eeprom_start(void)
|
||||||
|
|
||||||
if (ret < 0)
|
if (ret < 0)
|
||||||
errx(1, "failed to initialize NXFFS - erase EEPROM to reformat");
|
errx(1, "failed to initialize NXFFS - erase EEPROM to reformat");
|
||||||
|
|
||||||
/* mount the EEPROM */
|
/* mount the EEPROM */
|
||||||
ret = mount(NULL, "/eeprom", "nxffs", 0, NULL);
|
ret = mount(NULL, "/eeprom", "nxffs", 0, NULL);
|
||||||
|
|
||||||
if (ret < 0)
|
if (ret < 0)
|
||||||
errx(1, "failed to mount /eeprom - erase EEPROM to reformat");
|
errx(1, "failed to mount /eeprom - erase EEPROM to reformat");
|
||||||
|
|
||||||
started = true;
|
started = true;
|
||||||
warnx("mounted EEPROM at /eeprom");
|
warnx("mounted EEPROM at /eeprom");
|
||||||
exit(0);
|
exit(0);
|
||||||
|
@ -165,6 +165,7 @@ eeprom_erase(void)
|
||||||
|
|
||||||
if (at24c_nuke())
|
if (at24c_nuke())
|
||||||
errx(1, "erase failed");
|
errx(1, "erase failed");
|
||||||
|
|
||||||
errx(0, "erase done, reboot now");
|
errx(0, "erase done, reboot now");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -190,7 +191,7 @@ eeprom_save(const char *name)
|
||||||
if (!started)
|
if (!started)
|
||||||
errx(1, "must be started first");
|
errx(1, "must be started first");
|
||||||
|
|
||||||
if (!name)
|
if (!name)
|
||||||
err(1, "missing argument for device name, try '/eeprom/parameters'");
|
err(1, "missing argument for device name, try '/eeprom/parameters'");
|
||||||
|
|
||||||
warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead");
|
warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead");
|
||||||
|
@ -221,9 +222,9 @@ eeprom_load(const char *name)
|
||||||
if (!started)
|
if (!started)
|
||||||
errx(1, "must be started first");
|
errx(1, "must be started first");
|
||||||
|
|
||||||
if (!name)
|
if (!name)
|
||||||
err(1, "missing argument for device name, try '/eeprom/parameters'");
|
err(1, "missing argument for device name, try '/eeprom/parameters'");
|
||||||
|
|
||||||
warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead");
|
warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead");
|
||||||
|
|
||||||
int fd = open(name, O_RDONLY);
|
int fd = open(name, O_RDONLY);
|
||||||
|
|
|
@ -121,6 +121,7 @@ usage(const char *reason)
|
||||||
{
|
{
|
||||||
if (reason)
|
if (reason)
|
||||||
fprintf(stderr, "%s\n", reason);
|
fprintf(stderr, "%s\n", reason);
|
||||||
|
|
||||||
fprintf(stderr, "usage: led {start|stop|status} [-d <UART>]\n\n");
|
fprintf(stderr, "usage: led {start|stop|status} [-d <UART>]\n\n");
|
||||||
exit(1);
|
exit(1);
|
||||||
}
|
}
|
||||||
|
@ -129,7 +130,7 @@ usage(const char *reason)
|
||||||
* The deamon app only briefly exists to start
|
* The deamon app only briefly exists to start
|
||||||
* the background job. The stack size assigned in the
|
* the background job. The stack size assigned in the
|
||||||
* Makefile does only apply to this management task.
|
* Makefile does only apply to this management task.
|
||||||
*
|
*
|
||||||
* The actual stack size should be set in the call
|
* The actual stack size should be set in the call
|
||||||
* to task_create().
|
* to task_create().
|
||||||
*/
|
*/
|
||||||
|
@ -165,9 +166,11 @@ int led_main(int argc, char *argv[])
|
||||||
if (!strcmp(argv[1], "status")) {
|
if (!strcmp(argv[1], "status")) {
|
||||||
if (thread_running) {
|
if (thread_running) {
|
||||||
printf("\tled is running\n");
|
printf("\tled is running\n");
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
printf("\tled not started\n");
|
printf("\tled not started\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
exit(0);
|
exit(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -187,7 +190,7 @@ int led_thread_main(int argc, char *argv[])
|
||||||
|
|
||||||
while (!thread_should_exit) {
|
while (!thread_should_exit) {
|
||||||
/* swell blue led */
|
/* swell blue led */
|
||||||
|
|
||||||
|
|
||||||
/* 200 Hz base loop */
|
/* 200 Hz base loop */
|
||||||
usleep(1000000 / rate);
|
usleep(1000000 / rate);
|
||||||
|
|
|
@ -97,6 +97,7 @@ load(const char *devname, const char *fname)
|
||||||
|
|
||||||
/* tell it to load the file */
|
/* tell it to load the file */
|
||||||
ret = ioctl(dev, MIXERIOCLOADFILE, (unsigned long)fname);
|
ret = ioctl(dev, MIXERIOCLOADFILE, (unsigned long)fname);
|
||||||
|
|
||||||
if (ret != 0) {
|
if (ret != 0) {
|
||||||
fprintf(stderr, "failed loading %s\n", fname);
|
fprintf(stderr, "failed loading %s\n", fname);
|
||||||
}
|
}
|
||||||
|
|
|
@ -70,10 +70,13 @@ param_main(int argc, char *argv[])
|
||||||
if (argc >= 2) {
|
if (argc >= 2) {
|
||||||
if (!strcmp(argv[1], "save"))
|
if (!strcmp(argv[1], "save"))
|
||||||
do_save();
|
do_save();
|
||||||
|
|
||||||
if (!strcmp(argv[1], "load"))
|
if (!strcmp(argv[1], "load"))
|
||||||
do_load();
|
do_load();
|
||||||
|
|
||||||
if (!strcmp(argv[1], "import"))
|
if (!strcmp(argv[1], "import"))
|
||||||
do_import();
|
do_import();
|
||||||
|
|
||||||
if (!strcmp(argv[1], "show"))
|
if (!strcmp(argv[1], "show"))
|
||||||
do_show();
|
do_show();
|
||||||
}
|
}
|
||||||
|
@ -154,8 +157,8 @@ do_show_print(void *arg, param_t param)
|
||||||
float f;
|
float f;
|
||||||
|
|
||||||
printf("%c %s: ",
|
printf("%c %s: ",
|
||||||
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
|
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
|
||||||
param_name(param));
|
param_name(param));
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* This case can be expanded to handle printing common structure types.
|
* This case can be expanded to handle printing common structure types.
|
||||||
|
@ -167,19 +170,25 @@ do_show_print(void *arg, param_t param)
|
||||||
printf("%d\n", i);
|
printf("%d\n", i);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case PARAM_TYPE_FLOAT:
|
case PARAM_TYPE_FLOAT:
|
||||||
if (!param_get(param, &f)) {
|
if (!param_get(param, &f)) {
|
||||||
printf("%4.4f\n", (double)f);
|
printf("%4.4f\n", (double)f);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
|
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
|
||||||
printf("<struct type %d size %u>\n", 0 + param_type(param), param_size(param));
|
printf("<struct type %d size %u>\n", 0 + param_type(param), param_size(param));
|
||||||
return;
|
return;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
printf("<unknown type %d>\n", 0 + param_type(param));
|
printf("<unknown type %d>\n", 0 + param_type(param));
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
printf("<error fetching parameter %d>\n", param);
|
printf("<error fetching parameter %d>\n", param);
|
||||||
}
|
}
|
||||||
|
|
|
@ -190,13 +190,15 @@ int top_main(int argc, char *argv[])
|
||||||
runtime_spaces = "";
|
runtime_spaces = "";
|
||||||
}
|
}
|
||||||
|
|
||||||
unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
|
unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
|
||||||
(uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
|
(uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
|
||||||
unsigned stack_free = 0;
|
unsigned stack_free = 0;
|
||||||
uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
|
uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
|
||||||
|
|
||||||
while (stack_free < stack_size) {
|
while (stack_free < stack_size) {
|
||||||
if (*stack_sweeper++ != 0xff)
|
if (*stack_sweeper++ != 0xff)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
stack_free++;
|
stack_free++;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -56,7 +56,7 @@
|
||||||
static int
|
static int
|
||||||
read_int8(bson_decoder_t decoder, int8_t *b)
|
read_int8(bson_decoder_t decoder, int8_t *b)
|
||||||
{
|
{
|
||||||
return (read(decoder->fd, b, sizeof(*b)) == sizeof(*b)) ? 0 : -1;
|
return (read(decoder->fd, b, sizeof(*b)) == sizeof(*b)) ? 0 : -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
static int
|
static int
|
||||||
|
@ -119,12 +119,14 @@ bson_decoder_next(bson_decoder_t decoder)
|
||||||
while (decoder->pending > 0) {
|
while (decoder->pending > 0) {
|
||||||
if (read_int8(decoder, &tbyte))
|
if (read_int8(decoder, &tbyte))
|
||||||
CODER_KILL(decoder, "read error discarding pending bytes");
|
CODER_KILL(decoder, "read error discarding pending bytes");
|
||||||
|
|
||||||
decoder->pending--;
|
decoder->pending--;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* get the type byte */
|
/* get the type byte */
|
||||||
if (read_int8(decoder, &tbyte))
|
if (read_int8(decoder, &tbyte))
|
||||||
CODER_KILL(decoder, "read error on type byte");
|
CODER_KILL(decoder, "read error on type byte");
|
||||||
|
|
||||||
decoder->node.type = tbyte;
|
decoder->node.type = tbyte;
|
||||||
decoder->pending = 0;
|
decoder->pending = 0;
|
||||||
|
|
||||||
|
@ -135,13 +137,17 @@ bson_decoder_next(bson_decoder_t decoder)
|
||||||
|
|
||||||
/* get the node name */
|
/* get the node name */
|
||||||
nlen = 0;
|
nlen = 0;
|
||||||
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
if (nlen >= BSON_MAXNAME)
|
if (nlen >= BSON_MAXNAME)
|
||||||
CODER_KILL(decoder, "node name overflow");
|
CODER_KILL(decoder, "node name overflow");
|
||||||
|
|
||||||
if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen]))
|
if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen]))
|
||||||
CODER_KILL(decoder, "read error on node name");
|
CODER_KILL(decoder, "read error on node name");
|
||||||
|
|
||||||
if (decoder->node.name[nlen] == '\0')
|
if (decoder->node.name[nlen] == '\0')
|
||||||
break;
|
break;
|
||||||
|
|
||||||
nlen++;
|
nlen++;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -151,20 +157,28 @@ bson_decoder_next(bson_decoder_t decoder)
|
||||||
case BSON_INT:
|
case BSON_INT:
|
||||||
if (read_int32(decoder, &decoder->node.i))
|
if (read_int32(decoder, &decoder->node.i))
|
||||||
CODER_KILL(decoder, "read error on BSON_INT");
|
CODER_KILL(decoder, "read error on BSON_INT");
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case BSON_DOUBLE:
|
case BSON_DOUBLE:
|
||||||
if (read_double(decoder, &decoder->node.d))
|
if (read_double(decoder, &decoder->node.d))
|
||||||
CODER_KILL(decoder, "read error on BSON_DOUBLE");
|
CODER_KILL(decoder, "read error on BSON_DOUBLE");
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case BSON_STRING:
|
case BSON_STRING:
|
||||||
if (read_int32(decoder, &decoder->pending))
|
if (read_int32(decoder, &decoder->pending))
|
||||||
CODER_KILL(decoder, "read error on BSON_STRING length");
|
CODER_KILL(decoder, "read error on BSON_STRING length");
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case BSON_BINDATA:
|
case BSON_BINDATA:
|
||||||
if (read_int32(decoder, &decoder->pending))
|
if (read_int32(decoder, &decoder->pending))
|
||||||
CODER_KILL(decoder, "read error on BSON_BINDATA size");
|
CODER_KILL(decoder, "read error on BSON_BINDATA size");
|
||||||
|
|
||||||
if (read_int8(decoder, &tbyte))
|
if (read_int8(decoder, &tbyte))
|
||||||
CODER_KILL(decoder, "read error on BSON_BINDATA subtype");
|
CODER_KILL(decoder, "read error on BSON_BINDATA subtype");
|
||||||
|
|
||||||
decoder->node.subtype = tbyte;
|
decoder->node.subtype = tbyte;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
@ -186,11 +200,12 @@ bson_decoder_copy_data(bson_decoder_t decoder, void *buf)
|
||||||
CODER_CHECK(decoder);
|
CODER_CHECK(decoder);
|
||||||
|
|
||||||
/* if data already copied, return zero bytes */
|
/* if data already copied, return zero bytes */
|
||||||
if (decoder->pending == 0)
|
if (decoder->pending == 0)
|
||||||
return 0;
|
return 0;
|
||||||
|
|
||||||
/* copy bytes per the node size */
|
/* copy bytes per the node size */
|
||||||
result = read(decoder->fd, buf, decoder->pending);
|
result = read(decoder->fd, buf, decoder->pending);
|
||||||
|
|
||||||
if (result != decoder->pending)
|
if (result != decoder->pending)
|
||||||
CODER_KILL(decoder, "read error on copy_data");
|
CODER_KILL(decoder, "read error on copy_data");
|
||||||
|
|
||||||
|
@ -209,7 +224,7 @@ static int
|
||||||
write_int8(bson_encoder_t encoder, int8_t b)
|
write_int8(bson_encoder_t encoder, int8_t b)
|
||||||
{
|
{
|
||||||
debug("write_int8 %d", b);
|
debug("write_int8 %d", b);
|
||||||
return (write(encoder->fd, &b, sizeof(b)) == sizeof(b)) ? 0 : -1;
|
return (write(encoder->fd, &b, sizeof(b)) == sizeof(b)) ? 0 : -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
static int
|
static int
|
||||||
|
@ -233,6 +248,7 @@ write_name(bson_encoder_t encoder, const char *name)
|
||||||
|
|
||||||
if (len > BSON_MAXNAME)
|
if (len > BSON_MAXNAME)
|
||||||
return -1;
|
return -1;
|
||||||
|
|
||||||
debug("write name '%s' len %d", name, len);
|
debug("write name '%s' len %d", name, len);
|
||||||
return (write(encoder->fd, name, len + 1) == (int)(len + 1)) ? 0 : -1;
|
return (write(encoder->fd, name, len + 1) == (int)(len + 1)) ? 0 : -1;
|
||||||
}
|
}
|
||||||
|
@ -300,6 +316,7 @@ bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char
|
||||||
write_int32(encoder, len) ||
|
write_int32(encoder, len) ||
|
||||||
write(encoder->fd, name, len + 1) != (int)(len + 1))
|
write(encoder->fd, name, len + 1) != (int)(len + 1))
|
||||||
CODER_KILL(encoder, "write error on BSON_STRING");
|
CODER_KILL(encoder, "write error on BSON_STRING");
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -314,5 +331,6 @@ bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary
|
||||||
write_int8(encoder, subtype) ||
|
write_int8(encoder, subtype) ||
|
||||||
write(encoder->fd, data, size) != (int)(size))
|
write(encoder->fd, data, size) != (int)(size))
|
||||||
CODER_KILL(encoder, "write error on BSON_BINDATA");
|
CODER_KILL(encoder, "write error on BSON_BINDATA");
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
|
@ -31,14 +31,14 @@
|
||||||
*
|
*
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file tinybson.h
|
* @file tinybson.h
|
||||||
*
|
*
|
||||||
* A simple subset SAX-style BSON parser and generator. See http://bsonspec.org
|
* A simple subset SAX-style BSON parser and generator. See http://bsonspec.org
|
||||||
*
|
*
|
||||||
* Some types and defines taken from the standalone BSON parser/generator
|
* Some types and defines taken from the standalone BSON parser/generator
|
||||||
* in the Mongo C connector.
|
* in the Mongo C connector.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef _TINYBSON_H
|
#ifndef _TINYBSON_H
|
||||||
#define _TINYBSON_H
|
#define _TINYBSON_H
|
||||||
|
@ -77,8 +77,7 @@ typedef enum bson_binary_subtype {
|
||||||
/**
|
/**
|
||||||
* Node structure passed to the callback.
|
* Node structure passed to the callback.
|
||||||
*/
|
*/
|
||||||
typedef struct bson_node_s
|
typedef struct bson_node_s {
|
||||||
{
|
|
||||||
char name[BSON_MAXNAME];
|
char name[BSON_MAXNAME];
|
||||||
bson_type_t type;
|
bson_type_t type;
|
||||||
bson_binary_subtype_t subtype;
|
bson_binary_subtype_t subtype;
|
||||||
|
@ -96,8 +95,7 @@ typedef struct bson_decoder_s *bson_decoder_t;
|
||||||
*/
|
*/
|
||||||
typedef int (* bson_decoder_callback)(bson_decoder_t decoder, void *private, bson_node_t node);
|
typedef int (* bson_decoder_callback)(bson_decoder_t decoder, void *private, bson_node_t node);
|
||||||
|
|
||||||
struct bson_decoder_s
|
struct bson_decoder_s {
|
||||||
{
|
|
||||||
int fd;
|
int fd;
|
||||||
bson_decoder_callback callback;
|
bson_decoder_callback callback;
|
||||||
void *private;
|
void *private;
|
||||||
|
@ -143,8 +141,7 @@ __EXPORT size_t bson_decoder_data_pending(bson_decoder_t decoder);
|
||||||
/**
|
/**
|
||||||
* Encoder state structure.
|
* Encoder state structure.
|
||||||
*/
|
*/
|
||||||
typedef struct bson_encoder_s
|
typedef struct bson_encoder_s {
|
||||||
{
|
|
||||||
int fd;
|
int fd;
|
||||||
|
|
||||||
} *bson_encoder_t;
|
} *bson_encoder_t;
|
||||||
|
@ -169,7 +166,7 @@ __EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, i
|
||||||
*/
|
*/
|
||||||
__EXPORT int bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value);
|
__EXPORT int bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Append a string to the encoded stream.
|
* Append a string to the encoded stream.
|
||||||
*/
|
*/
|
||||||
__EXPORT int bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string);
|
__EXPORT int bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string);
|
||||||
|
|
|
@ -45,13 +45,13 @@
|
||||||
int16_t
|
int16_t
|
||||||
int16_t_from_bytes(uint8_t bytes[])
|
int16_t_from_bytes(uint8_t bytes[])
|
||||||
{
|
{
|
||||||
union {
|
union {
|
||||||
uint8_t b[2];
|
uint8_t b[2];
|
||||||
int16_t w;
|
int16_t w;
|
||||||
} u;
|
} u;
|
||||||
|
|
||||||
u.b[1] = bytes[0];
|
u.b[1] = bytes[0];
|
||||||
u.b[0] = bytes[1];
|
u.b[0] = bytes[1];
|
||||||
|
|
||||||
return u.w;
|
return u.w;
|
||||||
}
|
}
|
||||||
|
|
|
@ -79,43 +79,43 @@ void cpuload_initialize_once()
|
||||||
{
|
{
|
||||||
// if (!system_load.initialized)
|
// if (!system_load.initialized)
|
||||||
// {
|
// {
|
||||||
system_load.start_time = hrt_absolute_time();
|
system_load.start_time = hrt_absolute_time();
|
||||||
int i;
|
int i;
|
||||||
for (i = 0; i < CONFIG_MAX_TASKS; i++)
|
|
||||||
{
|
|
||||||
system_load.tasks[i].valid = false;
|
|
||||||
}
|
|
||||||
system_load.total_count = 0;
|
|
||||||
|
|
||||||
uint64_t now = hrt_absolute_time();
|
for (i = 0; i < CONFIG_MAX_TASKS; i++) {
|
||||||
|
system_load.tasks[i].valid = false;
|
||||||
|
}
|
||||||
|
|
||||||
/* initialize idle thread statically */
|
system_load.total_count = 0;
|
||||||
system_load.tasks[0].start_time = now;
|
|
||||||
system_load.tasks[0].total_runtime = 0;
|
|
||||||
system_load.tasks[0].curr_start_time = 0;
|
|
||||||
system_load.tasks[0].tcb = sched_gettcb(0);
|
|
||||||
system_load.tasks[0].valid = true;
|
|
||||||
system_load.total_count++;
|
|
||||||
|
|
||||||
/* initialize init thread statically */
|
uint64_t now = hrt_absolute_time();
|
||||||
system_load.tasks[1].start_time = now;
|
|
||||||
system_load.tasks[1].total_runtime = 0;
|
/* initialize idle thread statically */
|
||||||
system_load.tasks[1].curr_start_time = 0;
|
system_load.tasks[0].start_time = now;
|
||||||
system_load.tasks[1].tcb = sched_gettcb(1);
|
system_load.tasks[0].total_runtime = 0;
|
||||||
system_load.tasks[1].valid = true;
|
system_load.tasks[0].curr_start_time = 0;
|
||||||
/* count init thread */
|
system_load.tasks[0].tcb = sched_gettcb(0);
|
||||||
system_load.total_count++;
|
system_load.tasks[0].valid = true;
|
||||||
// }
|
system_load.total_count++;
|
||||||
|
|
||||||
|
/* initialize init thread statically */
|
||||||
|
system_load.tasks[1].start_time = now;
|
||||||
|
system_load.tasks[1].total_runtime = 0;
|
||||||
|
system_load.tasks[1].curr_start_time = 0;
|
||||||
|
system_load.tasks[1].tcb = sched_gettcb(1);
|
||||||
|
system_load.tasks[1].valid = true;
|
||||||
|
/* count init thread */
|
||||||
|
system_load.total_count++;
|
||||||
|
// }
|
||||||
}
|
}
|
||||||
|
|
||||||
void sched_note_start(FAR _TCB *tcb )
|
void sched_note_start(FAR _TCB *tcb)
|
||||||
{
|
{
|
||||||
/* search first free slot */
|
/* search first free slot */
|
||||||
int i;
|
int i;
|
||||||
for (i = 1; i < CONFIG_MAX_TASKS; i++)
|
|
||||||
{
|
for (i = 1; i < CONFIG_MAX_TASKS; i++) {
|
||||||
if (!system_load.tasks[i].valid)
|
if (!system_load.tasks[i].valid) {
|
||||||
{
|
|
||||||
/* slot is available */
|
/* slot is available */
|
||||||
system_load.tasks[i].start_time = hrt_absolute_time();
|
system_load.tasks[i].start_time = hrt_absolute_time();
|
||||||
system_load.tasks[i].total_runtime = 0;
|
system_load.tasks[i].total_runtime = 0;
|
||||||
|
@ -128,13 +128,12 @@ void sched_note_start(FAR _TCB *tcb )
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void sched_note_stop(FAR _TCB *tcb )
|
void sched_note_stop(FAR _TCB *tcb)
|
||||||
{
|
{
|
||||||
int i;
|
int i;
|
||||||
for (i = 1; i < CONFIG_MAX_TASKS; i++)
|
|
||||||
{
|
for (i = 1; i < CONFIG_MAX_TASKS; i++) {
|
||||||
if (system_load.tasks[i].tcb->pid == tcb->pid)
|
if (system_load.tasks[i].tcb->pid == tcb->pid) {
|
||||||
{
|
|
||||||
/* mark slot as fee */
|
/* mark slot as fee */
|
||||||
system_load.tasks[i].valid = false;
|
system_load.tasks[i].valid = false;
|
||||||
system_load.tasks[i].total_runtime = 0;
|
system_load.tasks[i].total_runtime = 0;
|
||||||
|
@ -152,26 +151,23 @@ void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb)
|
||||||
|
|
||||||
/* Kind of inefficient: find both tasks and update times */
|
/* Kind of inefficient: find both tasks and update times */
|
||||||
uint8_t both_found = 0;
|
uint8_t both_found = 0;
|
||||||
for (int i = 0; i < CONFIG_MAX_TASKS; i++)
|
|
||||||
{
|
for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
|
||||||
/* Task ending its current scheduling run */
|
/* Task ending its current scheduling run */
|
||||||
if (system_load.tasks[i].tcb->pid == pFromTcb->pid)
|
if (system_load.tasks[i].tcb->pid == pFromTcb->pid) {
|
||||||
{
|
|
||||||
//if (system_load.tasks[i].curr_start_time != 0)
|
//if (system_load.tasks[i].curr_start_time != 0)
|
||||||
{
|
{
|
||||||
system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time;
|
system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time;
|
||||||
}
|
}
|
||||||
both_found++;
|
both_found++;
|
||||||
}
|
|
||||||
else if (system_load.tasks[i].tcb->pid == pToTcb->pid)
|
} else if (system_load.tasks[i].tcb->pid == pToTcb->pid) {
|
||||||
{
|
|
||||||
system_load.tasks[i].curr_start_time = new_time;
|
system_load.tasks[i].curr_start_time = new_time;
|
||||||
both_found++;
|
both_found++;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Do only iterate as long as needed */
|
/* Do only iterate as long as needed */
|
||||||
if (both_found == 2)
|
if (both_found == 2) {
|
||||||
{
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -79,8 +79,10 @@ warnerr_core(int errcode, const char *fmt, va_list args)
|
||||||
/* convenience as many parts of NuttX use negative errno */
|
/* convenience as many parts of NuttX use negative errno */
|
||||||
if (errcode < 0)
|
if (errcode < 0)
|
||||||
errcode = -errcode;
|
errcode = -errcode;
|
||||||
|
|
||||||
if (errcode < NOCODE)
|
if (errcode < NOCODE)
|
||||||
fprintf(stderr, ": %s", strerror(errcode));
|
fprintf(stderr, ": %s", strerror(errcode));
|
||||||
|
|
||||||
fprintf(stderr, "\n");
|
fprintf(stderr, "\n");
|
||||||
#elif CONFIG_ARCH_LOWPUTC
|
#elif CONFIG_ARCH_LOWPUTC
|
||||||
lib_lowprintf("%s: ", getprogname());
|
lib_lowprintf("%s: ", getprogname());
|
||||||
|
@ -89,8 +91,10 @@ warnerr_core(int errcode, const char *fmt, va_list args)
|
||||||
/* convenience as many parts of NuttX use negative errno */
|
/* convenience as many parts of NuttX use negative errno */
|
||||||
if (errcode < 0)
|
if (errcode < 0)
|
||||||
errcode = -errcode;
|
errcode = -errcode;
|
||||||
|
|
||||||
if (errcode < NOCODE)
|
if (errcode < NOCODE)
|
||||||
lib_lowprintf(": %s", strerror(errcode));
|
lib_lowprintf(": %s", strerror(errcode));
|
||||||
|
|
||||||
lib_lowprintf("\n");
|
lib_lowprintf("\n");
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
@ -144,7 +148,7 @@ verrx(int exitcode, const char *fmt, va_list args)
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
warn(const char *fmt, ...)
|
warn(const char *fmt, ...)
|
||||||
{
|
{
|
||||||
va_list args;
|
va_list args;
|
||||||
|
|
||||||
|
@ -153,13 +157,13 @@ warn(const char *fmt, ...)
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
vwarn(const char *fmt, va_list args)
|
vwarn(const char *fmt, va_list args)
|
||||||
{
|
{
|
||||||
warnerr_core(errno, fmt, args);
|
warnerr_core(errno, fmt, args);
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
warnc(int errcode, const char *fmt, ...)
|
warnc(int errcode, const char *fmt, ...)
|
||||||
{
|
{
|
||||||
va_list args;
|
va_list args;
|
||||||
|
|
||||||
|
@ -168,13 +172,13 @@ warnc(int errcode, const char *fmt, ...)
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
vwarnc(int errcode, const char *fmt, va_list args)
|
vwarnc(int errcode, const char *fmt, va_list args)
|
||||||
{
|
{
|
||||||
warnerr_core(errcode, fmt, args);
|
warnerr_core(errcode, fmt, args);
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
warnx(const char *fmt, ...)
|
warnx(const char *fmt, ...)
|
||||||
{
|
{
|
||||||
va_list args;
|
va_list args;
|
||||||
|
|
||||||
|
@ -183,7 +187,7 @@ warnx(const char *fmt, ...)
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
vwarnx(const char *fmt, va_list args)
|
vwarnx(const char *fmt, va_list args)
|
||||||
{
|
{
|
||||||
warnerr_core(NOCODE, fmt, args);
|
warnerr_core(NOCODE, fmt, args);
|
||||||
}
|
}
|
||||||
|
|
|
@ -36,30 +36,30 @@
|
||||||
*
|
*
|
||||||
* Simple error/warning functions, heavily inspired by the BSD functions of
|
* Simple error/warning functions, heavily inspired by the BSD functions of
|
||||||
* the same names.
|
* the same names.
|
||||||
*
|
*
|
||||||
* The err() and warn() family of functions display a formatted error
|
* The err() and warn() family of functions display a formatted error
|
||||||
* message on the standard error output. In all cases, the last
|
* message on the standard error output. In all cases, the last
|
||||||
* component of the program name, a colon character, and a space are
|
* component of the program name, a colon character, and a space are
|
||||||
* output. If the fmt argument is not NULL, the printf(3)-like formatted
|
* output. If the fmt argument is not NULL, the printf(3)-like formatted
|
||||||
* error message is output. The output is terminated by a newline
|
* error message is output. The output is terminated by a newline
|
||||||
* character.
|
* character.
|
||||||
*
|
*
|
||||||
* The err(), errc(), verr(), verrc(), warn(), warnc(), vwarn(), and
|
* The err(), errc(), verr(), verrc(), warn(), warnc(), vwarn(), and
|
||||||
* vwarnc() functions append an error message obtained from strerror(3)
|
* vwarnc() functions append an error message obtained from strerror(3)
|
||||||
* based on a supplied error code value or the global variable errno,
|
* based on a supplied error code value or the global variable errno,
|
||||||
* preceded by another colon and space unless the fmt argument is NULL.
|
* preceded by another colon and space unless the fmt argument is NULL.
|
||||||
*
|
*
|
||||||
* In the case of the errc(), verrc(), warnc(), and vwarnc() functions,
|
* In the case of the errc(), verrc(), warnc(), and vwarnc() functions,
|
||||||
* the code argument is used to look up the error message.
|
* the code argument is used to look up the error message.
|
||||||
*
|
*
|
||||||
* The err(), verr(), warn(), and vwarn() functions use the global
|
* The err(), verr(), warn(), and vwarn() functions use the global
|
||||||
* variable errno to look up the error message.
|
* variable errno to look up the error message.
|
||||||
*
|
*
|
||||||
* The errx() and warnx() functions do not append an error message.
|
* The errx() and warnx() functions do not append an error message.
|
||||||
*
|
*
|
||||||
* The err(), verr(), errc(), verrc(), errx(), and verrx() functions do
|
* The err(), verr(), errc(), verrc(), errx(), and verrx() functions do
|
||||||
* not return, but exit with the value of the argument eval.
|
* not return, but exit with the value of the argument eval.
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef _SYSTEMLIB_ERR_H
|
#ifndef _SYSTEMLIB_ERR_H
|
||||||
|
@ -71,18 +71,18 @@ __BEGIN_DECLS
|
||||||
|
|
||||||
__EXPORT const char *getprogname(void);
|
__EXPORT const char *getprogname(void);
|
||||||
|
|
||||||
__EXPORT void err(int eval, const char *fmt, ...) __attribute__((noreturn,format(printf,2, 3)));
|
__EXPORT void err(int eval, const char *fmt, ...) __attribute__((noreturn, format(printf, 2, 3)));
|
||||||
__EXPORT void verr(int eval, const char *fmt, va_list) __attribute__((noreturn,format(printf,2, 0)));
|
__EXPORT void verr(int eval, const char *fmt, va_list) __attribute__((noreturn, format(printf, 2, 0)));
|
||||||
__EXPORT void errc(int eval, int code, const char *fmt, ...) __attribute__((noreturn,format(printf,3, 4)));
|
__EXPORT void errc(int eval, int code, const char *fmt, ...) __attribute__((noreturn, format(printf, 3, 4)));
|
||||||
__EXPORT void verrc(int eval, int code, const char *fmt, va_list) __attribute__((noreturn,format(printf,3, 0)));
|
__EXPORT void verrc(int eval, int code, const char *fmt, va_list) __attribute__((noreturn, format(printf, 3, 0)));
|
||||||
__EXPORT void errx(int eval, const char *fmt, ...) __attribute__((noreturn,format(printf,2, 3)));
|
__EXPORT void errx(int eval, const char *fmt, ...) __attribute__((noreturn, format(printf, 2, 3)));
|
||||||
__EXPORT void verrx(int eval, const char *fmt, va_list) __attribute__((noreturn,format(printf,2, 0)));
|
__EXPORT void verrx(int eval, const char *fmt, va_list) __attribute__((noreturn, format(printf, 2, 0)));
|
||||||
__EXPORT void warn(const char *fmt, ...) __attribute__((format(printf,1, 2)));
|
__EXPORT void warn(const char *fmt, ...) __attribute__((format(printf, 1, 2)));
|
||||||
__EXPORT void vwarn(const char *fmt, va_list) __attribute__((format(printf,1, 0)));
|
__EXPORT void vwarn(const char *fmt, va_list) __attribute__((format(printf, 1, 0)));
|
||||||
__EXPORT void warnc(int code, const char *fmt, ...) __attribute__((format(printf,2, 3)));
|
__EXPORT void warnc(int code, const char *fmt, ...) __attribute__((format(printf, 2, 3)));
|
||||||
__EXPORT void vwarnc(int code, const char *fmt, va_list) __attribute__((format(printf,2, 0)));
|
__EXPORT void vwarnc(int code, const char *fmt, va_list) __attribute__((format(printf, 2, 0)));
|
||||||
__EXPORT void warnx(const char *fmt, ...) __attribute__((format(printf,1, 2)));
|
__EXPORT void warnx(const char *fmt, ...) __attribute__((format(printf, 1, 2)));
|
||||||
__EXPORT void vwarnx(const char *fmt, va_list) __attribute__((format(printf,1, 0)));
|
__EXPORT void vwarnx(const char *fmt, va_list) __attribute__((format(printf, 1, 0)));
|
||||||
|
|
||||||
__END_DECLS
|
__END_DECLS
|
||||||
|
|
||||||
|
|
|
@ -89,7 +89,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
|
||||||
__EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
|
__EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
|
||||||
{
|
{
|
||||||
// This function returns the distance to the nearest point on the track line. Distance is positive if current
|
// This function returns the distance to the nearest point on the track line. Distance is positive if current
|
||||||
// position is right of the track and negative if left of the track as seen from a point on the track line
|
// position is right of the track and negative if left of the track as seen from a point on the track line
|
||||||
// headed towards the end point.
|
// headed towards the end point.
|
||||||
|
|
||||||
crosstrack_error_s return_var;
|
crosstrack_error_s return_var;
|
||||||
|
@ -97,43 +97,46 @@ __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now,
|
||||||
float bearing_end;
|
float bearing_end;
|
||||||
float bearing_track;
|
float bearing_track;
|
||||||
float bearing_diff;
|
float bearing_diff;
|
||||||
|
|
||||||
return_var.error = true; // Set error flag, cleared when valid result calculated.
|
return_var.error = true; // Set error flag, cleared when valid result calculated.
|
||||||
return_var.past_end = false;
|
return_var.past_end = false;
|
||||||
return_var.distance = 0.0f;
|
return_var.distance = 0.0f;
|
||||||
return_var.bearing = 0.0f;
|
return_var.bearing = 0.0f;
|
||||||
|
|
||||||
// Return error if arguments are bad
|
// Return error if arguments are bad
|
||||||
if(lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var;
|
if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var;
|
||||||
|
|
||||||
bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
||||||
bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
|
bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
|
||||||
bearing_diff = bearing_track - bearing_end;
|
bearing_diff = bearing_track - bearing_end;
|
||||||
bearing_diff = _wrapPI(bearing_diff);
|
bearing_diff = _wrapPI(bearing_diff);
|
||||||
|
|
||||||
// Return past_end = true if past end point of line
|
// Return past_end = true if past end point of line
|
||||||
if(bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
|
if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
|
||||||
return_var.past_end = true;
|
return_var.past_end = true;
|
||||||
return_var.error = false;
|
return_var.error = false;
|
||||||
return return_var;
|
return return_var;
|
||||||
}
|
}
|
||||||
|
|
||||||
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
||||||
return_var.distance = (dist_to_end)*sin(bearing_diff);
|
return_var.distance = (dist_to_end) * sin(bearing_diff);
|
||||||
if(sin(bearing_diff) >=0 ) {
|
|
||||||
|
if (sin(bearing_diff) >= 0) {
|
||||||
return_var.bearing = _wrapPI(bearing_track - M_PI_2_F);
|
return_var.bearing = _wrapPI(bearing_track - M_PI_2_F);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
return_var.bearing = _wrapPI(bearing_track + M_PI_2_F);
|
return_var.bearing = _wrapPI(bearing_track + M_PI_2_F);
|
||||||
}
|
}
|
||||||
|
|
||||||
return_var.error = false;
|
return_var.error = false;
|
||||||
|
|
||||||
return return_var;
|
return return_var;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center,
|
__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center,
|
||||||
float radius, float arc_start_bearing, float arc_sweep)
|
float radius, float arc_start_bearing, float arc_sweep)
|
||||||
{
|
{
|
||||||
// This function returns the distance to the nearest point on the track arc. Distance is positive if current
|
// This function returns the distance to the nearest point on the track arc. Distance is positive if current
|
||||||
// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
|
// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
|
||||||
|
@ -146,68 +149,76 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now,
|
||||||
float bearing_sector_end;
|
float bearing_sector_end;
|
||||||
float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
|
float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
|
||||||
bool in_sector;
|
bool in_sector;
|
||||||
|
|
||||||
return_var.error = true; // Set error flag, cleared when valid result calculated.
|
return_var.error = true; // Set error flag, cleared when valid result calculated.
|
||||||
return_var.past_end = false;
|
return_var.past_end = false;
|
||||||
return_var.distance = 0.0f;
|
return_var.distance = 0.0f;
|
||||||
return_var.bearing = 0.0f;
|
return_var.bearing = 0.0f;
|
||||||
|
|
||||||
// Return error if arguments are bad
|
// Return error if arguments are bad
|
||||||
if(lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var;
|
if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var;
|
||||||
|
|
||||||
|
|
||||||
if(arc_sweep >= 0) {
|
if (arc_sweep >= 0) {
|
||||||
bearing_sector_start = arc_start_bearing;
|
bearing_sector_start = arc_start_bearing;
|
||||||
bearing_sector_end = arc_start_bearing + arc_sweep;
|
bearing_sector_end = arc_start_bearing + arc_sweep;
|
||||||
if(bearing_sector_end > 2.0f*M_PI_F) bearing_sector_end -= M_TWOPI_F;
|
|
||||||
|
if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
bearing_sector_end = arc_start_bearing;
|
bearing_sector_end = arc_start_bearing;
|
||||||
bearing_sector_start = arc_start_bearing - arc_sweep;
|
bearing_sector_start = arc_start_bearing - arc_sweep;
|
||||||
if(bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F;
|
|
||||||
|
if (bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F;
|
||||||
}
|
}
|
||||||
|
|
||||||
in_sector = false;
|
in_sector = false;
|
||||||
|
|
||||||
// Case where sector does not span zero
|
// Case where sector does not span zero
|
||||||
if(bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true;
|
if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true;
|
||||||
|
|
||||||
// Case where sector does span zero
|
// Case where sector does span zero
|
||||||
if(bearing_sector_end < bearing_sector_start && ( bearing_now > bearing_sector_start || bearing_now < bearing_sector_end ) ) in_sector = true;
|
if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true;
|
||||||
|
|
||||||
// If in the sector then calculate distance and bearing to closest point
|
// If in the sector then calculate distance and bearing to closest point
|
||||||
if(in_sector) {
|
if (in_sector) {
|
||||||
return_var.past_end = false;
|
return_var.past_end = false;
|
||||||
float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
|
float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
|
||||||
|
|
||||||
if(dist_to_center <= radius) {
|
if (dist_to_center <= radius) {
|
||||||
return_var.distance = radius - dist_to_center;
|
return_var.distance = radius - dist_to_center;
|
||||||
return_var.bearing = bearing_now + M_PI_F;
|
return_var.bearing = bearing_now + M_PI_F;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
return_var.distance = dist_to_center - radius;
|
return_var.distance = dist_to_center - radius;
|
||||||
return_var.bearing = bearing_now;
|
return_var.bearing = bearing_now;
|
||||||
}
|
}
|
||||||
|
|
||||||
// If out of the sector then calculate dist and bearing to start or end point
|
// If out of the sector then calculate dist and bearing to start or end point
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
||||||
// Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude)
|
// Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude)
|
||||||
// and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to
|
// and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to
|
||||||
// calculate the position of the start and end points. We should not be doing this often
|
// calculate the position of the start and end points. We should not be doing this often
|
||||||
// as this function generally will not be called repeatedly when we are out of the sector.
|
// as this function generally will not be called repeatedly when we are out of the sector.
|
||||||
|
|
||||||
// TO DO - this is messed up and won't compile
|
// TO DO - this is messed up and won't compile
|
||||||
float start_disp_x = radius * sin(arc_start_bearing);
|
float start_disp_x = radius * sin(arc_start_bearing);
|
||||||
float start_disp_y = radius * cos(arc_start_bearing);
|
float start_disp_y = radius * cos(arc_start_bearing);
|
||||||
float end_disp_x = radius * sin(_wrapPI(arc_start_bearing+arc_sweep));
|
float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep));
|
||||||
float end_disp_y = radius * cos(_wrapPI(arc_start_bearing+arc_sweep));
|
float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep));
|
||||||
float lon_start = lon_now + start_disp_x/111111.0d;
|
float lon_start = lon_now + start_disp_x / 111111.0d;
|
||||||
float lat_start = lat_now + start_disp_y*cos(lat_now)/111111.0d;
|
float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d;
|
||||||
float lon_end = lon_now + end_disp_x/111111.0d;
|
float lon_end = lon_now + end_disp_x / 111111.0d;
|
||||||
float lat_end = lat_now + end_disp_y*cos(lat_now)/111111.0d;
|
float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d;
|
||||||
float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
|
float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
|
||||||
float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
||||||
if(dist_to_start < dist_to_end) {
|
|
||||||
|
if (dist_to_start < dist_to_end) {
|
||||||
return_var.distance = dist_to_start;
|
return_var.distance = dist_to_start;
|
||||||
return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
|
return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
return_var.past_end = true;
|
return_var.past_end = true;
|
||||||
return_var.distance = dist_to_end;
|
return_var.distance = dist_to_end;
|
||||||
|
@ -215,10 +226,10 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now,
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return_var.bearing = _wrapPI(return_var.bearing);
|
return_var.bearing = _wrapPI(return_var.bearing);
|
||||||
return_var.error = false;
|
return_var.error = false;
|
||||||
return return_var;
|
return return_var;
|
||||||
}
|
}
|
||||||
|
|
||||||
float _wrapPI(float bearing)
|
float _wrapPI(float bearing)
|
||||||
|
@ -227,21 +238,25 @@ float _wrapPI(float bearing)
|
||||||
while (bearing > M_PI_F) {
|
while (bearing > M_PI_F) {
|
||||||
bearing = bearing - M_TWOPI_F;
|
bearing = bearing - M_TWOPI_F;
|
||||||
}
|
}
|
||||||
|
|
||||||
while (bearing <= -M_PI_F) {
|
while (bearing <= -M_PI_F) {
|
||||||
bearing = bearing + M_TWOPI_F;
|
bearing = bearing + M_TWOPI_F;
|
||||||
}
|
}
|
||||||
|
|
||||||
return bearing;
|
return bearing;
|
||||||
}
|
}
|
||||||
|
|
||||||
float _wrap2PI(float bearing)
|
float _wrap2PI(float bearing)
|
||||||
{
|
{
|
||||||
|
|
||||||
while (bearing >= M_TWOPI_F) {
|
while (bearing >= M_TWOPI_F) {
|
||||||
bearing = bearing - M_TWOPI_F;
|
bearing = bearing - M_TWOPI_F;
|
||||||
}
|
}
|
||||||
|
|
||||||
while (bearing < 0.0f) {
|
while (bearing < 0.0f) {
|
||||||
bearing = bearing + M_TWOPI_F;
|
bearing = bearing + M_TWOPI_F;
|
||||||
}
|
}
|
||||||
|
|
||||||
return bearing;
|
return bearing;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -251,23 +266,26 @@ float _wrap180(float bearing)
|
||||||
while (bearing > 180.0f) {
|
while (bearing > 180.0f) {
|
||||||
bearing = bearing - 360.0f;
|
bearing = bearing - 360.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
while (bearing <= -180.0f) {
|
while (bearing <= -180.0f) {
|
||||||
bearing = bearing + 360.0f;
|
bearing = bearing + 360.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
return bearing;
|
return bearing;
|
||||||
}
|
}
|
||||||
|
|
||||||
float _wrap360(float bearing)
|
float _wrap360(float bearing)
|
||||||
{
|
{
|
||||||
|
|
||||||
while (bearing >= 360.0f) {
|
while (bearing >= 360.0f) {
|
||||||
bearing = bearing - 360.0f;
|
bearing = bearing - 360.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
while (bearing < 0.0f) {
|
while (bearing < 0.0f) {
|
||||||
bearing = bearing + 360.0f;
|
bearing = bearing + 360.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
return bearing;
|
return bearing;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -44,8 +44,8 @@
|
||||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||||
* Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
|
* Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
|
@ -59,14 +59,14 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
|
||||||
|
|
||||||
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
|
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
|
||||||
|
|
||||||
//
|
//
|
||||||
|
|
||||||
__EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
|
__EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
|
||||||
|
|
||||||
__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center,
|
__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center,
|
||||||
float radius, float arc_start_bearing, float arc_sweep);
|
float radius, float arc_start_bearing, float arc_sweep);
|
||||||
|
|
||||||
float _wrap180(float bearing);
|
float _wrap180(float bearing);
|
||||||
float _wrap360(float bearing);
|
float _wrap360(float bearing);
|
||||||
float _wrapPI(float bearing);
|
float _wrapPI(float bearing);
|
||||||
float _wrap2PI(float bearing);
|
float _wrap2PI(float bearing);
|
|
@ -63,7 +63,7 @@ float
|
||||||
Mixer::get_control(uint8_t group, uint8_t index)
|
Mixer::get_control(uint8_t group, uint8_t index)
|
||||||
{
|
{
|
||||||
float value;
|
float value;
|
||||||
|
|
||||||
_control_cb(_cb_handle, group, index, value);
|
_control_cb(_cb_handle, group, index, value);
|
||||||
|
|
||||||
return value;
|
return value;
|
||||||
|
|
|
@ -224,29 +224,35 @@ mixer_load_multirotor(Mixer::ControlCallback control_cb, uintptr_t cb_handle, co
|
||||||
|
|
||||||
if (!strcmp(geomname, "4+")) {
|
if (!strcmp(geomname, "4+")) {
|
||||||
geometry = MultirotorMixer::QUAD_PLUS;
|
geometry = MultirotorMixer::QUAD_PLUS;
|
||||||
|
|
||||||
} else if (!strcmp(geomname, "4x")) {
|
} else if (!strcmp(geomname, "4x")) {
|
||||||
geometry = MultirotorMixer::QUAD_X;
|
geometry = MultirotorMixer::QUAD_X;
|
||||||
|
|
||||||
} else if (!strcmp(geomname, "6+")) {
|
} else if (!strcmp(geomname, "6+")) {
|
||||||
geometry = MultirotorMixer::HEX_PLUS;
|
geometry = MultirotorMixer::HEX_PLUS;
|
||||||
|
|
||||||
} else if (!strcmp(geomname, "6x")) {
|
} else if (!strcmp(geomname, "6x")) {
|
||||||
geometry = MultirotorMixer::HEX_X;
|
geometry = MultirotorMixer::HEX_X;
|
||||||
|
|
||||||
} else if (!strcmp(geomname, "8+")) {
|
} else if (!strcmp(geomname, "8+")) {
|
||||||
geometry = MultirotorMixer::OCTA_PLUS;
|
geometry = MultirotorMixer::OCTA_PLUS;
|
||||||
|
|
||||||
} else if (!strcmp(geomname, "8x")) {
|
} else if (!strcmp(geomname, "8x")) {
|
||||||
geometry = MultirotorMixer::OCTA_X;
|
geometry = MultirotorMixer::OCTA_X;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
debug("unrecognised geometry '%s'", geomname);
|
debug("unrecognised geometry '%s'", geomname);
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
return new MultirotorMixer(
|
return new MultirotorMixer(
|
||||||
control_cb,
|
control_cb,
|
||||||
cb_handle,
|
cb_handle,
|
||||||
geometry,
|
geometry,
|
||||||
s[0] / 10000.0f,
|
s[0] / 10000.0f,
|
||||||
s[1] / 10000.0f,
|
s[1] / 10000.0f,
|
||||||
s[2] / 10000.0f,
|
s[2] / 10000.0f,
|
||||||
s[3] / 10000.0f);
|
s[3] / 10000.0f);
|
||||||
}
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
|
|
|
@ -57,7 +57,7 @@
|
||||||
#define CW (-1.0f)
|
#define CW (-1.0f)
|
||||||
#define CCW (1.0f)
|
#define CCW (1.0f)
|
||||||
|
|
||||||
namespace
|
namespace
|
||||||
{
|
{
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -167,17 +167,21 @@ MultirotorMixer::mix(float *outputs, unsigned space)
|
||||||
pitch * _rotors[i].pitch_scale +
|
pitch * _rotors[i].pitch_scale +
|
||||||
yaw * _rotors[i].yaw_scale +
|
yaw * _rotors[i].yaw_scale +
|
||||||
thrust;
|
thrust;
|
||||||
|
|
||||||
if (tmp > max)
|
if (tmp > max)
|
||||||
max = tmp;
|
max = tmp;
|
||||||
|
|
||||||
outputs[i] = tmp;
|
outputs[i] = tmp;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* scale values into the -1.0 - 1.0 range */
|
/* scale values into the -1.0 - 1.0 range */
|
||||||
if (max > 1.0f) {
|
if (max > 1.0f) {
|
||||||
fixup_scale = 2.0f / max;
|
fixup_scale = 2.0f / max;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
fixup_scale = 2.0f;
|
fixup_scale = 2.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
for (unsigned i = 0; i < _rotor_count; i++)
|
for (unsigned i = 0; i < _rotor_count; i++)
|
||||||
outputs[i] = -1.0f + (outputs[i] * fixup_scale);
|
outputs[i] = -1.0f + (outputs[i] * fixup_scale);
|
||||||
|
|
||||||
|
|
|
@ -189,6 +189,7 @@ param_notify_changes(void)
|
||||||
*/
|
*/
|
||||||
if (param_topic == -1) {
|
if (param_topic == -1) {
|
||||||
param_topic = orb_advertise(ORB_ID(parameter_update), &pup);
|
param_topic = orb_advertise(ORB_ID(parameter_update), &pup);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
orb_publish(ORB_ID(parameter_update), param_topic, &pup);
|
orb_publish(ORB_ID(parameter_update), param_topic, &pup);
|
||||||
}
|
}
|
||||||
|
@ -455,6 +456,7 @@ param_reset(param_t param)
|
||||||
utarray_erase(param_values, pos, 1);
|
utarray_erase(param_values, pos, 1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
param_unlock();
|
param_unlock();
|
||||||
|
|
||||||
if (s != NULL)
|
if (s != NULL)
|
||||||
|
@ -560,8 +562,7 @@ out:
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
struct param_import_state
|
struct param_import_state {
|
||||||
{
|
|
||||||
bool mark_saved;
|
bool mark_saved;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -689,9 +690,10 @@ param_import_internal(int fd, bool mark_saved)
|
||||||
do {
|
do {
|
||||||
result = bson_decoder_next(&decoder);
|
result = bson_decoder_next(&decoder);
|
||||||
|
|
||||||
} while(result > 0);
|
} while (result > 0);
|
||||||
|
|
||||||
out:
|
out:
|
||||||
|
|
||||||
if (result < 0)
|
if (result < 0)
|
||||||
debug("BSON error decoding parameters");
|
debug("BSON error decoding parameters");
|
||||||
|
|
||||||
|
|
|
@ -43,7 +43,7 @@
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
|
||||||
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
|
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
|
||||||
float limit, uint8_t mode)
|
float limit, uint8_t mode)
|
||||||
{
|
{
|
||||||
pid->kp = kp;
|
pid->kp = kp;
|
||||||
pid->ki = ki;
|
pid->ki = ki;
|
||||||
|
@ -65,30 +65,35 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
|
||||||
|
|
||||||
if (isfinite(kp)) {
|
if (isfinite(kp)) {
|
||||||
pid->kp = kp;
|
pid->kp = kp;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
ret = 1;
|
ret = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (isfinite(ki)) {
|
if (isfinite(ki)) {
|
||||||
pid->ki = ki;
|
pid->ki = ki;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
ret = 1;
|
ret = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (isfinite(kd)) {
|
if (isfinite(kd)) {
|
||||||
pid->kd = kd;
|
pid->kd = kd;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
ret = 1;
|
ret = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (isfinite(intmax)) {
|
if (isfinite(intmax)) {
|
||||||
pid->intmax = intmax;
|
pid->intmax = intmax;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
ret = 1;
|
ret = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (isfinite(limit)) {
|
if (isfinite(limit)) {
|
||||||
pid->limit = limit;
|
pid->limit = limit;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
ret = 1;
|
ret = 1;
|
||||||
}
|
}
|
||||||
|
@ -121,17 +126,16 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
|
||||||
goto start
|
goto start
|
||||||
*/
|
*/
|
||||||
|
|
||||||
if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt))
|
if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) {
|
||||||
{
|
|
||||||
return pid->last_output;
|
return pid->last_output;
|
||||||
}
|
}
|
||||||
|
|
||||||
float i, d;
|
float i, d;
|
||||||
pid->sp = sp;
|
pid->sp = sp;
|
||||||
|
|
||||||
// Calculated current error value
|
// Calculated current error value
|
||||||
float error = pid->sp - val;
|
float error = pid->sp - val;
|
||||||
|
|
||||||
if (isfinite(error)) { // Why is this necessary? DEW
|
if (isfinite(error)) { // Why is this necessary? DEW
|
||||||
pid->error_previous = error;
|
pid->error_previous = error;
|
||||||
}
|
}
|
||||||
|
@ -140,30 +144,36 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
|
||||||
|
|
||||||
if (pid->mode == PID_MODE_DERIVATIV_CALC) {
|
if (pid->mode == PID_MODE_DERIVATIV_CALC) {
|
||||||
d = (error - pid->error_previous) / dt;
|
d = (error - pid->error_previous) / dt;
|
||||||
|
|
||||||
} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
|
} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
|
||||||
d = -val_dot;
|
d = -val_dot;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
d = 0.0f;
|
d = 0.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Calculate the error integral and check for saturation
|
// Calculate the error integral and check for saturation
|
||||||
i = pid->integral + (error * dt);
|
i = pid->integral + (error * dt);
|
||||||
if( fabs((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit ||
|
|
||||||
fabs(i) > pid->intmax )
|
if (fabs((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit ||
|
||||||
{
|
fabs(i) > pid->intmax) {
|
||||||
i = pid->integral; // If saturated then do not update integral value
|
i = pid->integral; // If saturated then do not update integral value
|
||||||
pid->saturated = 1;
|
pid->saturated = 1;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (!isfinite(i)) {
|
if (!isfinite(i)) {
|
||||||
i = 0;
|
i = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
pid->integral = i;
|
pid->integral = i;
|
||||||
pid->saturated = 0;
|
pid->saturated = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Calculate the output. Limit output magnitude to pid->limit
|
// Calculate the output. Limit output magnitude to pid->limit
|
||||||
float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd);
|
float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd);
|
||||||
|
|
||||||
if (output > pid->limit) output = pid->limit;
|
if (output > pid->limit) output = pid->limit;
|
||||||
|
|
||||||
if (output < -pid->limit) output = -pid->limit;
|
if (output < -pid->limit) output = -pid->limit;
|
||||||
|
|
||||||
if (isfinite(output)) {
|
if (isfinite(output)) {
|
||||||
|
|
|
@ -30,7 +30,7 @@
|
||||||
* POSSIBILITY OF SUCH DAMAGE.
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
*
|
*
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file ppm_decode.c
|
* @file ppm_decode.c
|
||||||
*
|
*
|
||||||
|
@ -51,7 +51,7 @@
|
||||||
*
|
*
|
||||||
* The PPM decoder works as follows.
|
* The PPM decoder works as follows.
|
||||||
*
|
*
|
||||||
* Initially, the decoder waits in the UNSYNCH state for two edges
|
* Initially, the decoder waits in the UNSYNCH state for two edges
|
||||||
* separated by PPM_MIN_START. Once the second edge is detected,
|
* separated by PPM_MIN_START. Once the second edge is detected,
|
||||||
* the decoder moves to the ARM state.
|
* the decoder moves to the ARM state.
|
||||||
*
|
*
|
||||||
|
@ -64,9 +64,9 @@
|
||||||
*
|
*
|
||||||
* The ACTIVE state expects an edge within PPM_MAX_PULSE_WIDTH, and when
|
* The ACTIVE state expects an edge within PPM_MAX_PULSE_WIDTH, and when
|
||||||
* received calculates the time from the previous mark and records
|
* received calculates the time from the previous mark and records
|
||||||
* this time as the value for the next channel.
|
* this time as the value for the next channel.
|
||||||
*
|
*
|
||||||
* If at any time waiting for an edge, the delay from the previous edge
|
* If at any time waiting for an edge, the delay from the previous edge
|
||||||
* exceeds PPM_MIN_START the frame is deemed to have ended and the recorded
|
* exceeds PPM_MIN_START the frame is deemed to have ended and the recorded
|
||||||
* values are advertised to clients.
|
* values are advertised to clients.
|
||||||
*/
|
*/
|
||||||
|
@ -132,21 +132,23 @@ ppm_input_decode(bool reset, unsigned count)
|
||||||
|
|
||||||
/* how long since the last edge? */
|
/* how long since the last edge? */
|
||||||
width = count - ppm.last_edge;
|
width = count - ppm.last_edge;
|
||||||
|
|
||||||
if (count < ppm.last_edge)
|
if (count < ppm.last_edge)
|
||||||
width += ppm.count_max; /* handle wrapped count */
|
width += ppm.count_max; /* handle wrapped count */
|
||||||
|
|
||||||
ppm.last_edge = count;
|
ppm.last_edge = count;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* If this looks like a start pulse, then push the last set of values
|
* If this looks like a start pulse, then push the last set of values
|
||||||
* and reset the state machine.
|
* and reset the state machine.
|
||||||
*
|
*
|
||||||
* Note that this is not a "high performance" design; it implies a whole
|
* Note that this is not a "high performance" design; it implies a whole
|
||||||
* frame of latency between the pulses being received and their being
|
* frame of latency between the pulses being received and their being
|
||||||
* considered valid.
|
* considered valid.
|
||||||
*/
|
*/
|
||||||
if (width >= PPM_MIN_START) {
|
if (width >= PPM_MIN_START) {
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* If the number of channels changes unexpectedly, we don't want
|
* If the number of channels changes unexpectedly, we don't want
|
||||||
* to just immediately jump on the new count as it may be a result
|
* to just immediately jump on the new count as it may be a result
|
||||||
* of noise or dropped edges. Instead, take a few frames to settle.
|
* of noise or dropped edges. Instead, take a few frames to settle.
|
||||||
|
@ -169,11 +171,13 @@ ppm_input_decode(bool reset, unsigned count)
|
||||||
ppm_decoded_channels = new_channel_count;
|
ppm_decoded_channels = new_channel_count;
|
||||||
new_channel_count = 0;
|
new_channel_count = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
/* frame channel count matches expected, let's use it */
|
/* frame channel count matches expected, let's use it */
|
||||||
if (ppm.next_channel > PPM_MIN_CHANNELS) {
|
if (ppm.next_channel > PPM_MIN_CHANNELS) {
|
||||||
for (i = 0; i < ppm.next_channel; i++)
|
for (i = 0; i < ppm.next_channel; i++)
|
||||||
ppm_buffer[i] = ppm_temp_buffer[i];
|
ppm_buffer[i] = ppm_temp_buffer[i];
|
||||||
|
|
||||||
ppm_last_valid_decode = hrt_absolute_time();
|
ppm_last_valid_decode = hrt_absolute_time();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -193,10 +197,11 @@ ppm_input_decode(bool reset, unsigned count)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
case ARM:
|
case ARM:
|
||||||
|
|
||||||
/* we expect a pulse giving us the first mark */
|
/* we expect a pulse giving us the first mark */
|
||||||
if (width > PPM_MAX_PULSE_WIDTH)
|
if (width > PPM_MAX_PULSE_WIDTH)
|
||||||
goto error; /* pulse was too long */
|
goto error; /* pulse was too long */
|
||||||
|
|
||||||
/* record the mark timing, expect an inactive edge */
|
/* record the mark timing, expect an inactive edge */
|
||||||
ppm.last_mark = count;
|
ppm.last_mark = count;
|
||||||
ppm.phase = INACTIVE;
|
ppm.phase = INACTIVE;
|
||||||
|
@ -211,6 +216,7 @@ ppm_input_decode(bool reset, unsigned count)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
case ACTIVE:
|
case ACTIVE:
|
||||||
|
|
||||||
/* we expect a well-formed pulse */
|
/* we expect a well-formed pulse */
|
||||||
if (width > PPM_MAX_PULSE_WIDTH)
|
if (width > PPM_MAX_PULSE_WIDTH)
|
||||||
goto error; /* pulse was too long */
|
goto error; /* pulse was too long */
|
||||||
|
@ -228,7 +234,7 @@ ppm_input_decode(bool reset, unsigned count)
|
||||||
ppm_temp_buffer[ppm.next_channel++] = interval;
|
ppm_temp_buffer[ppm.next_channel++] = interval;
|
||||||
|
|
||||||
ppm.phase = INACTIVE;
|
ppm.phase = INACTIVE;
|
||||||
return;
|
return;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -30,7 +30,7 @@
|
||||||
* POSSIBILITY OF SUCH DAMAGE.
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
*
|
*
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file ppm_decode.h
|
* @file ppm_decode.h
|
||||||
*
|
*
|
||||||
|
|
|
@ -131,7 +131,7 @@ int task_spawn(const char *name, int scheduler, int priority, int stack_size, ma
|
||||||
pid = task_create(name, priority, stack_size, entry, argv);
|
pid = task_create(name, priority, stack_size, entry, argv);
|
||||||
|
|
||||||
if (pid > 0) {
|
if (pid > 0) {
|
||||||
|
|
||||||
/* configure the scheduler */
|
/* configure the scheduler */
|
||||||
struct sched_param param;
|
struct sched_param param;
|
||||||
|
|
||||||
|
@ -140,6 +140,7 @@ int task_spawn(const char *name, int scheduler, int priority, int stack_size, ma
|
||||||
|
|
||||||
/* XXX do any other private task accounting here before the task starts */
|
/* XXX do any other private task accounting here before the task starts */
|
||||||
}
|
}
|
||||||
|
|
||||||
sched_unlock();
|
sched_unlock();
|
||||||
|
|
||||||
return pid;
|
return pid;
|
||||||
|
@ -157,15 +158,18 @@ int fmu_get_board_info(struct fmu_board_info_s *info)
|
||||||
statres = stat("/dev/bma180", &sb);
|
statres = stat("/dev/bma180", &sb);
|
||||||
|
|
||||||
if (statres == OK) {
|
if (statres == OK) {
|
||||||
/* BMA180 indicates a v1.5-v1.6 board */
|
/* BMA180 indicates a v1.5-v1.6 board */
|
||||||
strcpy(info->board_name, "FMU v1.6");
|
strcpy(info->board_name, "FMU v1.6");
|
||||||
info->board_version = 16;
|
info->board_version = 16;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
statres = stat("/dev/accel", &sb);
|
statres = stat("/dev/accel", &sb);
|
||||||
|
|
||||||
if (statres == OK) {
|
if (statres == OK) {
|
||||||
/* MPU-6000 indicates a v1.7+ board */
|
/* MPU-6000 indicates a v1.7+ board */
|
||||||
strcpy(info->board_name, "FMU v1.7");
|
strcpy(info->board_name, "FMU v1.7");
|
||||||
info->board_version = 17;
|
info->board_version = 17;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
/* If no BMA and no MPU is present, it is a v1.3 board */
|
/* If no BMA and no MPU is present, it is a v1.3 board */
|
||||||
strcpy(info->board_name, "FMU v1.3");
|
strcpy(info->board_name, "FMU v1.3");
|
||||||
|
|
Loading…
Reference in New Issue