mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-15 11:03:35 +08:00
Merged master
This commit is contained in:
Lorenz Meier
+236
-172
@@ -48,161 +48,44 @@
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#define DEBUG
|
||||
|
||||
#include "px4io.h"
|
||||
|
||||
#define DSM_FRAME_SIZE 16
|
||||
#define DSM_FRAME_CHANNELS 7
|
||||
#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/
|
||||
#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/
|
||||
|
||||
static int dsm_fd = -1;
|
||||
|
||||
static hrt_abstime last_rx_time;
|
||||
static hrt_abstime last_frame_time;
|
||||
|
||||
static uint8_t frame[DSM_FRAME_SIZE];
|
||||
|
||||
static unsigned partial_frame_count;
|
||||
static unsigned channel_shift;
|
||||
|
||||
unsigned dsm_frame_drops;
|
||||
|
||||
static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
|
||||
static void dsm_guess_format(bool reset);
|
||||
static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values);
|
||||
|
||||
int
|
||||
dsm_init(const char *device)
|
||||
{
|
||||
if (dsm_fd < 0)
|
||||
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
|
||||
|
||||
if (dsm_fd >= 0) {
|
||||
struct termios t;
|
||||
|
||||
/* 115200bps, no parity, one stop bit */
|
||||
tcgetattr(dsm_fd, &t);
|
||||
cfsetspeed(&t, 115200);
|
||||
t.c_cflag &= ~(CSTOPB | PARENB);
|
||||
tcsetattr(dsm_fd, TCSANOW, &t);
|
||||
|
||||
/* initialise the decoder */
|
||||
partial_frame_count = 0;
|
||||
last_rx_time = hrt_absolute_time();
|
||||
|
||||
/* reset the format detector */
|
||||
dsm_guess_format(true);
|
||||
|
||||
debug("DSM: ready");
|
||||
|
||||
} else {
|
||||
debug("DSM: open failed");
|
||||
}
|
||||
|
||||
return dsm_fd;
|
||||
}
|
||||
|
||||
void
|
||||
dsm_bind(uint16_t cmd, int pulses)
|
||||
{
|
||||
const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10;
|
||||
|
||||
if (dsm_fd < 0)
|
||||
return;
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
|
||||
// XXX implement
|
||||
#warning DSM BIND NOT IMPLEMENTED ON PX4IO V2
|
||||
#else
|
||||
switch (cmd) {
|
||||
case dsm_bind_power_down:
|
||||
// power down DSM satellite
|
||||
POWER_RELAY1(0);
|
||||
break;
|
||||
case dsm_bind_power_up:
|
||||
POWER_RELAY1(1);
|
||||
dsm_guess_format(true);
|
||||
break;
|
||||
case dsm_bind_set_rx_out:
|
||||
stm32_configgpio(usart1RxAsOutp);
|
||||
break;
|
||||
case dsm_bind_send_pulses:
|
||||
for (int i = 0; i < pulses; i++) {
|
||||
stm32_gpiowrite(usart1RxAsOutp, false);
|
||||
up_udelay(25);
|
||||
stm32_gpiowrite(usart1RxAsOutp, true);
|
||||
up_udelay(25);
|
||||
}
|
||||
break;
|
||||
case dsm_bind_reinit_uart:
|
||||
// Restore USART rx pin
|
||||
stm32_configgpio(GPIO_USART1_RX);
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
bool
|
||||
dsm_input(uint16_t *values, uint16_t *num_values)
|
||||
{
|
||||
ssize_t ret;
|
||||
hrt_abstime now;
|
||||
|
||||
/*
|
||||
* The DSM* protocol doesn't provide any explicit framing,
|
||||
* so we detect frame boundaries by the inter-frame delay.
|
||||
*
|
||||
* The minimum frame spacing is 11ms; with 16 bytes at 115200bps
|
||||
* frame transmission time is ~1.4ms.
|
||||
*
|
||||
* We expect to only be called when bytes arrive for processing,
|
||||
* and if an interval of more than 5ms passes between calls,
|
||||
* the first byte we read will be the first byte of a frame.
|
||||
*
|
||||
* In the case where byte(s) are dropped from a frame, this also
|
||||
* provides a degree of protection. Of course, it would be better
|
||||
* if we didn't drop bytes...
|
||||
*/
|
||||
now = hrt_absolute_time();
|
||||
|
||||
if ((now - last_rx_time) > 5000) {
|
||||
if (partial_frame_count > 0) {
|
||||
dsm_frame_drops++;
|
||||
partial_frame_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Fetch bytes, but no more than we would need to complete
|
||||
* the current frame.
|
||||
*/
|
||||
ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count);
|
||||
|
||||
/* if the read failed for any reason, just give up here */
|
||||
if (ret < 1)
|
||||
return false;
|
||||
|
||||
last_rx_time = now;
|
||||
|
||||
/*
|
||||
* Add bytes to the current frame
|
||||
*/
|
||||
partial_frame_count += ret;
|
||||
|
||||
/*
|
||||
* If we don't have a full frame, return
|
||||
*/
|
||||
if (partial_frame_count < DSM_FRAME_SIZE)
|
||||
return false;
|
||||
|
||||
/*
|
||||
* Great, it looks like we might have a frame. Go ahead and
|
||||
* decode it.
|
||||
*/
|
||||
partial_frame_count = 0;
|
||||
return dsm_decode(now, values, num_values);
|
||||
}
|
||||
static int dsm_fd = -1; /**< File handle to the DSM UART */
|
||||
static hrt_abstime dsm_last_rx_time; /**< Timestamp when we last received */
|
||||
static hrt_abstime dsm_last_frame_time; /**< Timestamp for start of last dsm frame */
|
||||
static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**< DSM dsm frame receive buffer */
|
||||
static unsigned dsm_partial_frame_count; /**< Count of bytes received for current dsm frame */
|
||||
static unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
|
||||
static unsigned dsm_frame_drops; /**< Count of incomplete DSM frames */
|
||||
|
||||
/**
|
||||
* Attempt to decode a single channel raw channel datum
|
||||
*
|
||||
* The DSM* protocol doesn't provide any explicit framing,
|
||||
* so we detect dsm frame boundaries by the inter-dsm frame delay.
|
||||
*
|
||||
* The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
|
||||
* dsm frame transmission time is ~1.4ms.
|
||||
*
|
||||
* We expect to only be called when bytes arrive for processing,
|
||||
* and if an interval of more than 5ms passes between calls,
|
||||
* the first byte we read will be the first byte of a dsm frame.
|
||||
*
|
||||
* In the case where byte(s) are dropped from a dsm frame, this also
|
||||
* provides a degree of protection. Of course, it would be better
|
||||
* if we didn't drop bytes...
|
||||
*
|
||||
* Upon receiving a full dsm frame we attempt to decode it
|
||||
*
|
||||
* @param[in] raw 16 bit raw channel value from dsm frame
|
||||
* @param[in] shift position of channel number in raw data
|
||||
* @param[out] channel pointer to returned channel number
|
||||
* @param[out] value pointer to returned channel value
|
||||
* @return true=raw value successfully decoded
|
||||
*/
|
||||
static bool
|
||||
dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
|
||||
{
|
||||
@@ -220,6 +103,11 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Attempt to guess if receiving 10 or 11 bit channel values
|
||||
*
|
||||
* @param[in] reset true=reset the 10/11 bit state to unknown
|
||||
*/
|
||||
static void
|
||||
dsm_guess_format(bool reset)
|
||||
{
|
||||
@@ -232,14 +120,14 @@ dsm_guess_format(bool reset)
|
||||
cs10 = 0;
|
||||
cs11 = 0;
|
||||
samples = 0;
|
||||
channel_shift = 0;
|
||||
dsm_channel_shift = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
/* scan the channels in the current frame in both 10- and 11-bit mode */
|
||||
/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
|
||||
for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
|
||||
|
||||
uint8_t *dp = &frame[2 + (2 * i)];
|
||||
uint8_t *dp = &dsm_frame[2 + (2 * i)];
|
||||
uint16_t raw = (dp[0] << 8) | dp[1];
|
||||
unsigned channel, value;
|
||||
|
||||
@@ -250,10 +138,10 @@ dsm_guess_format(bool reset)
|
||||
if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
|
||||
cs11 |= (1 << channel);
|
||||
|
||||
/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */
|
||||
/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
|
||||
}
|
||||
|
||||
/* wait until we have seen plenty of frames - 2 should normally be enough */
|
||||
/* wait until we have seen plenty of frames - 5 should normally be enough */
|
||||
if (samples++ < 5)
|
||||
return;
|
||||
|
||||
@@ -289,13 +177,13 @@ dsm_guess_format(bool reset)
|
||||
}
|
||||
|
||||
if ((votes11 == 1) && (votes10 == 0)) {
|
||||
channel_shift = 11;
|
||||
dsm_channel_shift = 11;
|
||||
debug("DSM: 11-bit format");
|
||||
return;
|
||||
}
|
||||
|
||||
if ((votes10 == 1) && (votes11 == 0)) {
|
||||
channel_shift = 10;
|
||||
dsm_channel_shift = 10;
|
||||
debug("DSM: 10-bit format");
|
||||
return;
|
||||
}
|
||||
@@ -305,27 +193,136 @@ dsm_guess_format(bool reset)
|
||||
dsm_guess_format(true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize the DSM receive functionality
|
||||
*
|
||||
* Open the UART for receiving DSM frames and configure it appropriately
|
||||
*
|
||||
* @param[in] device Device name of DSM UART
|
||||
*/
|
||||
int
|
||||
dsm_init(const char *device)
|
||||
{
|
||||
if (dsm_fd < 0)
|
||||
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
|
||||
|
||||
if (dsm_fd >= 0) {
|
||||
|
||||
struct termios t;
|
||||
|
||||
/* 115200bps, no parity, one stop bit */
|
||||
tcgetattr(dsm_fd, &t);
|
||||
cfsetspeed(&t, 115200);
|
||||
t.c_cflag &= ~(CSTOPB | PARENB);
|
||||
tcsetattr(dsm_fd, TCSANOW, &t);
|
||||
|
||||
/* initialise the decoder */
|
||||
dsm_partial_frame_count = 0;
|
||||
dsm_last_rx_time = hrt_absolute_time();
|
||||
|
||||
/* reset the format detector */
|
||||
dsm_guess_format(true);
|
||||
|
||||
debug("DSM: ready");
|
||||
|
||||
} else {
|
||||
|
||||
debug("DSM: open failed");
|
||||
|
||||
}
|
||||
|
||||
return dsm_fd;
|
||||
}
|
||||
|
||||
/**
|
||||
* Handle DSM satellite receiver bind mode handler
|
||||
*
|
||||
* @param[in] cmd commands - dsm_bind_power_down, dsm_bind_power_up, dsm_bind_set_rx_out, dsm_bind_send_pulses, dsm_bind_reinit_uart
|
||||
* @param[in] pulses Number of pulses for dsm_bind_send_pulses command
|
||||
*/
|
||||
void
|
||||
dsm_bind(uint16_t cmd, int pulses)
|
||||
{
|
||||
const uint32_t usart1RxAsOutp =
|
||||
GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
|
||||
|
||||
if (dsm_fd < 0)
|
||||
return;
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
|
||||
// XXX implement
|
||||
#warning DSM BIND NOT IMPLEMENTED ON PX4IO V2
|
||||
#else
|
||||
switch (cmd) {
|
||||
|
||||
case dsm_bind_power_down:
|
||||
|
||||
/*power down DSM satellite*/
|
||||
POWER_RELAY1(0);
|
||||
break;
|
||||
|
||||
case dsm_bind_power_up:
|
||||
|
||||
/*power up DSM satellite*/
|
||||
POWER_RELAY1(1);
|
||||
dsm_guess_format(true);
|
||||
break;
|
||||
|
||||
case dsm_bind_set_rx_out:
|
||||
|
||||
/*Set UART RX pin to active output mode*/
|
||||
stm32_configgpio(usart1RxAsOutp);
|
||||
break;
|
||||
|
||||
case dsm_bind_send_pulses:
|
||||
|
||||
/*Pulse RX pin a number of times*/
|
||||
for (int i = 0; i < pulses; i++) {
|
||||
stm32_gpiowrite(usart1RxAsOutp, false);
|
||||
up_udelay(25);
|
||||
stm32_gpiowrite(usart1RxAsOutp, true);
|
||||
up_udelay(25);
|
||||
}
|
||||
break;
|
||||
|
||||
case dsm_bind_reinit_uart:
|
||||
|
||||
/*Restore USART RX pin to RS232 receive mode*/
|
||||
stm32_configgpio(GPIO_USART1_RX);
|
||||
break;
|
||||
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* Decode the entire dsm frame (all contained channels)
|
||||
*
|
||||
* @param[in] frame_time timestamp when this dsm frame was received. Used to detect RX loss in order to reset 10/11 bit guess.
|
||||
* @param[out] values pointer to per channel array of decoded values
|
||||
* @param[out] num_values pointer to number of raw channel values returned
|
||||
* @return true=DSM frame successfully decoded, false=no update
|
||||
*/
|
||||
static bool
|
||||
dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
|
||||
{
|
||||
|
||||
/*
|
||||
debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
|
||||
frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
|
||||
frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
|
||||
debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
|
||||
dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7],
|
||||
dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]);
|
||||
*/
|
||||
/*
|
||||
* If we have lost signal for at least a second, reset the
|
||||
* format guessing heuristic.
|
||||
*/
|
||||
if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
|
||||
if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0))
|
||||
dsm_guess_format(true);
|
||||
|
||||
/* we have received something we think is a frame */
|
||||
last_frame_time = frame_time;
|
||||
/* we have received something we think is a dsm_frame */
|
||||
dsm_last_frame_time = frame_time;
|
||||
|
||||
/* if we don't know the frame format, update the guessing state machine */
|
||||
if (channel_shift == 0) {
|
||||
/* if we don't know the dsm_frame format, update the guessing state machine */
|
||||
if (dsm_channel_shift == 0) {
|
||||
dsm_guess_format(false);
|
||||
return false;
|
||||
}
|
||||
@@ -337,17 +334,17 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
|
||||
* Each channel is a 16-bit unsigned value containing either a 10-
|
||||
* or 11-bit channel value and a 4-bit channel number, shifted
|
||||
* either 10 or 11 bits. The MSB may also be set to indicate the
|
||||
* second frame in variants of the protocol where more than
|
||||
* second dsm_frame in variants of the protocol where more than
|
||||
* seven channels are being transmitted.
|
||||
*/
|
||||
|
||||
for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
|
||||
|
||||
uint8_t *dp = &frame[2 + (2 * i)];
|
||||
uint8_t *dp = &dsm_frame[2 + (2 * i)];
|
||||
uint16_t raw = (dp[0] << 8) | dp[1];
|
||||
unsigned channel, value;
|
||||
|
||||
if (!dsm_decode_channel(raw, channel_shift, &channel, &value))
|
||||
if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value))
|
||||
continue;
|
||||
|
||||
/* ignore channels out of range */
|
||||
@@ -359,7 +356,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
|
||||
*num_values = channel + 1;
|
||||
|
||||
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
|
||||
if (channel_shift == 11)
|
||||
if (dsm_channel_shift == 11)
|
||||
value /= 2;
|
||||
|
||||
value += 998;
|
||||
@@ -390,7 +387,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
|
||||
values[channel] = value;
|
||||
}
|
||||
|
||||
if (channel_shift == 11)
|
||||
if (dsm_channel_shift == 11)
|
||||
*num_values |= 0x8000;
|
||||
|
||||
/*
|
||||
@@ -398,3 +395,70 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
|
||||
*/
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Called periodically to check for input data from the DSM UART
|
||||
*
|
||||
* The DSM* protocol doesn't provide any explicit framing,
|
||||
* so we detect dsm frame boundaries by the inter-dsm frame delay.
|
||||
* The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
|
||||
* dsm frame transmission time is ~1.4ms.
|
||||
* We expect to only be called when bytes arrive for processing,
|
||||
* and if an interval of more than 5ms passes between calls,
|
||||
* the first byte we read will be the first byte of a dsm frame.
|
||||
* In the case where byte(s) are dropped from a dsm frame, this also
|
||||
* provides a degree of protection. Of course, it would be better
|
||||
* if we didn't drop bytes...
|
||||
* Upon receiving a full dsm frame we attempt to decode it.
|
||||
*
|
||||
* @param[out] values pointer to per channel array of decoded values
|
||||
* @param[out] num_values pointer to number of raw channel values returned
|
||||
* @return true=decoded raw channel values updated, false=no update
|
||||
*/
|
||||
bool
|
||||
dsm_input(uint16_t *values, uint16_t *num_values)
|
||||
{
|
||||
ssize_t ret;
|
||||
hrt_abstime now;
|
||||
|
||||
/*
|
||||
*/
|
||||
now = hrt_absolute_time();
|
||||
|
||||
if ((now - dsm_last_rx_time) > 5000) {
|
||||
if (dsm_partial_frame_count > 0) {
|
||||
dsm_frame_drops++;
|
||||
dsm_partial_frame_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Fetch bytes, but no more than we would need to complete
|
||||
* the current dsm frame.
|
||||
*/
|
||||
ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count);
|
||||
|
||||
/* if the read failed for any reason, just give up here */
|
||||
if (ret < 1)
|
||||
return false;
|
||||
|
||||
dsm_last_rx_time = now;
|
||||
|
||||
/*
|
||||
* Add bytes to the current dsm frame
|
||||
*/
|
||||
dsm_partial_frame_count += ret;
|
||||
|
||||
/*
|
||||
* If we don't have a full dsm frame, return
|
||||
*/
|
||||
if (dsm_partial_frame_count < DSM_FRAME_SIZE)
|
||||
return false;
|
||||
|
||||
/*
|
||||
* Great, it looks like we might have a dsm frame. Go ahead and
|
||||
* decode it.
|
||||
*/
|
||||
dsm_partial_frame_count = 0;
|
||||
return dsm_decode(now, values, num_values);
|
||||
}
|
||||
|
||||
@@ -198,7 +198,7 @@ extern void controls_init(void);
|
||||
extern void controls_tick(void);
|
||||
extern int dsm_init(const char *device);
|
||||
extern bool dsm_input(uint16_t *values, uint16_t *num_values);
|
||||
extern void dsm_bind(uint16_t cmd, int pulses);
|
||||
extern void dsm_bind(uint16_t cmd, int pulses);
|
||||
extern int sbus_init(const char *device);
|
||||
extern bool sbus_input(uint16_t *values, uint16_t *num_values);
|
||||
|
||||
|
||||
@@ -70,6 +70,9 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0);
|
||||
|
||||
PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
|
||||
PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
|
||||
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
|
||||
PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
|
||||
|
||||
+144
-15
@@ -50,6 +50,7 @@
|
||||
#include <stdio.h>
|
||||
#include <errno.h>
|
||||
#include <math.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
#include <nuttx/analog/adc.h>
|
||||
|
||||
@@ -137,6 +138,77 @@
|
||||
|
||||
#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
|
||||
|
||||
/**
|
||||
* Enum for board and external compass rotations.
|
||||
* This enum maps from board attitude to airframe attitude.
|
||||
*/
|
||||
enum Rotation {
|
||||
ROTATION_NONE = 0,
|
||||
ROTATION_YAW_45 = 1,
|
||||
ROTATION_YAW_90 = 2,
|
||||
ROTATION_YAW_135 = 3,
|
||||
ROTATION_YAW_180 = 4,
|
||||
ROTATION_YAW_225 = 5,
|
||||
ROTATION_YAW_270 = 6,
|
||||
ROTATION_YAW_315 = 7,
|
||||
ROTATION_ROLL_180 = 8,
|
||||
ROTATION_ROLL_180_YAW_45 = 9,
|
||||
ROTATION_ROLL_180_YAW_90 = 10,
|
||||
ROTATION_ROLL_180_YAW_135 = 11,
|
||||
ROTATION_PITCH_180 = 12,
|
||||
ROTATION_ROLL_180_YAW_225 = 13,
|
||||
ROTATION_ROLL_180_YAW_270 = 14,
|
||||
ROTATION_ROLL_180_YAW_315 = 15,
|
||||
ROTATION_ROLL_90 = 16,
|
||||
ROTATION_ROLL_90_YAW_45 = 17,
|
||||
ROTATION_ROLL_90_YAW_90 = 18,
|
||||
ROTATION_ROLL_90_YAW_135 = 19,
|
||||
ROTATION_ROLL_270 = 20,
|
||||
ROTATION_ROLL_270_YAW_45 = 21,
|
||||
ROTATION_ROLL_270_YAW_90 = 22,
|
||||
ROTATION_ROLL_270_YAW_135 = 23,
|
||||
ROTATION_PITCH_90 = 24,
|
||||
ROTATION_PITCH_270 = 25,
|
||||
ROTATION_MAX
|
||||
};
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint16_t roll;
|
||||
uint16_t pitch;
|
||||
uint16_t yaw;
|
||||
} rot_lookup_t;
|
||||
|
||||
const rot_lookup_t rot_lookup[] =
|
||||
{
|
||||
{ 0, 0, 0 },
|
||||
{ 0, 0, 45 },
|
||||
{ 0, 0, 90 },
|
||||
{ 0, 0, 135 },
|
||||
{ 0, 0, 180 },
|
||||
{ 0, 0, 225 },
|
||||
{ 0, 0, 270 },
|
||||
{ 0, 0, 315 },
|
||||
{180, 0, 0 },
|
||||
{180, 0, 45 },
|
||||
{180, 0, 90 },
|
||||
{180, 0, 135 },
|
||||
{ 0, 180, 0 },
|
||||
{180, 0, 225 },
|
||||
{180, 0, 270 },
|
||||
{180, 0, 315 },
|
||||
{ 90, 0, 0 },
|
||||
{ 90, 0, 45 },
|
||||
{ 90, 0, 90 },
|
||||
{ 90, 0, 135 },
|
||||
{270, 0, 0 },
|
||||
{270, 0, 45 },
|
||||
{270, 0, 90 },
|
||||
{270, 0, 135 },
|
||||
{ 0, 90, 0 },
|
||||
{ 0, 270, 0 }
|
||||
};
|
||||
|
||||
/**
|
||||
* Sensor app start / stop handling function
|
||||
*
|
||||
@@ -189,8 +261,8 @@ private:
|
||||
int _mag_sub; /**< raw mag data subscription */
|
||||
int _rc_sub; /**< raw rc channels data subscription */
|
||||
int _baro_sub; /**< raw baro data subscription */
|
||||
int _airspeed_sub; /**< airspeed subscription */
|
||||
int _diff_pres_sub; /**< raw differential pressure subscription */
|
||||
int _airspeed_sub; /**< airspeed subscription */
|
||||
int _diff_pres_sub; /**< raw differential pressure subscription */
|
||||
int _vcontrol_mode_sub; /**< vehicle control mode subscription */
|
||||
int _params_sub; /**< notification of parameter updates */
|
||||
int _manual_control_sub; /**< notification of manual control updates */
|
||||
@@ -210,13 +282,15 @@ private:
|
||||
struct differential_pressure_s _diff_pres;
|
||||
struct airspeed_s _airspeed;
|
||||
|
||||
math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
|
||||
math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
|
||||
|
||||
struct {
|
||||
float min[_rc_max_chan_count];
|
||||
float trim[_rc_max_chan_count];
|
||||
float max[_rc_max_chan_count];
|
||||
float rev[_rc_max_chan_count];
|
||||
float dz[_rc_max_chan_count];
|
||||
// float ex[_rc_max_chan_count];
|
||||
float scaling_factor[_rc_max_chan_count];
|
||||
|
||||
float gyro_offset[3];
|
||||
@@ -227,6 +301,9 @@ private:
|
||||
float accel_scale[3];
|
||||
float diff_pres_offset_pa;
|
||||
|
||||
int board_rotation;
|
||||
int external_mag_rotation;
|
||||
|
||||
int rc_type;
|
||||
|
||||
int rc_map_roll;
|
||||
@@ -264,7 +341,6 @@ private:
|
||||
param_t max[_rc_max_chan_count];
|
||||
param_t rev[_rc_max_chan_count];
|
||||
param_t dz[_rc_max_chan_count];
|
||||
// param_t ex[_rc_max_chan_count];
|
||||
param_t rc_type;
|
||||
|
||||
param_t rc_demix;
|
||||
@@ -304,6 +380,9 @@ private:
|
||||
|
||||
param_t battery_voltage_scaling;
|
||||
|
||||
param_t board_rotation;
|
||||
param_t external_mag_rotation;
|
||||
|
||||
} _parameter_handles; /**< handles for interesting parameters */
|
||||
|
||||
|
||||
@@ -312,6 +391,11 @@ private:
|
||||
*/
|
||||
int parameters_update();
|
||||
|
||||
/**
|
||||
* Get the rotation matrices
|
||||
*/
|
||||
void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
|
||||
|
||||
/**
|
||||
* Do accel-related initialisation.
|
||||
*/
|
||||
@@ -448,7 +532,10 @@ Sensors::Sensors() :
|
||||
_diff_pres_pub(-1),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
|
||||
|
||||
_board_rotation(3,3),
|
||||
_external_mag_rotation(3,3)
|
||||
{
|
||||
|
||||
/* basic r/c parameters */
|
||||
@@ -538,6 +625,10 @@ Sensors::Sensors() :
|
||||
|
||||
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
|
||||
|
||||
/* rotations */
|
||||
_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
|
||||
_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
|
||||
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
}
|
||||
@@ -724,9 +815,33 @@ Sensors::parameters_update()
|
||||
warnx("Failed updating voltage scaling param");
|
||||
}
|
||||
|
||||
param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
|
||||
param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
|
||||
|
||||
get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
|
||||
get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
|
||||
{
|
||||
/* first set to zero */
|
||||
rot_matrix->Matrix::zero(3,3);
|
||||
|
||||
float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
|
||||
float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
|
||||
float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
|
||||
|
||||
math::EulerAngles euler(roll, pitch, yaw);
|
||||
|
||||
math::Dcm R(euler);
|
||||
|
||||
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
|
||||
(*rot_matrix)(i,j) = R(i, j);
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::accel_init()
|
||||
{
|
||||
@@ -750,7 +865,7 @@ Sensors::accel_init()
|
||||
/* set the driver to poll at 1000Hz */
|
||||
ioctl(fd, SENSORIOCSPOLLRATE, 1000);
|
||||
|
||||
#else
|
||||
#elif CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
|
||||
/* set the accel internal sampling rate up to at leat 800Hz */
|
||||
ioctl(fd, ACCELIOCSSAMPLERATE, 800);
|
||||
@@ -758,6 +873,9 @@ Sensors::accel_init()
|
||||
/* set the driver to poll at 800Hz */
|
||||
ioctl(fd, SENSORIOCSPOLLRATE, 800);
|
||||
|
||||
#else
|
||||
#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
|
||||
#endif
|
||||
|
||||
warnx("using system accel");
|
||||
@@ -867,9 +985,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
|
||||
|
||||
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
|
||||
|
||||
raw.accelerometer_m_s2[0] = accel_report.x;
|
||||
raw.accelerometer_m_s2[1] = accel_report.y;
|
||||
raw.accelerometer_m_s2[2] = accel_report.z;
|
||||
math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
|
||||
vect = _board_rotation*vect;
|
||||
|
||||
raw.accelerometer_m_s2[0] = vect(0);
|
||||
raw.accelerometer_m_s2[1] = vect(1);
|
||||
raw.accelerometer_m_s2[2] = vect(2);
|
||||
|
||||
raw.accelerometer_raw[0] = accel_report.x_raw;
|
||||
raw.accelerometer_raw[1] = accel_report.y_raw;
|
||||
@@ -890,9 +1011,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
|
||||
|
||||
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
|
||||
|
||||
raw.gyro_rad_s[0] = gyro_report.x;
|
||||
raw.gyro_rad_s[1] = gyro_report.y;
|
||||
raw.gyro_rad_s[2] = gyro_report.z;
|
||||
math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
|
||||
vect = _board_rotation*vect;
|
||||
|
||||
raw.gyro_rad_s[0] = vect(0);
|
||||
raw.gyro_rad_s[1] = vect(1);
|
||||
raw.gyro_rad_s[2] = vect(2);
|
||||
|
||||
raw.gyro_raw[0] = gyro_report.x_raw;
|
||||
raw.gyro_raw[1] = gyro_report.y_raw;
|
||||
@@ -913,9 +1037,14 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
|
||||
|
||||
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
|
||||
|
||||
raw.magnetometer_ga[0] = mag_report.x;
|
||||
raw.magnetometer_ga[1] = mag_report.y;
|
||||
raw.magnetometer_ga[2] = mag_report.z;
|
||||
// XXX TODO add support for external mag orientation
|
||||
|
||||
math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
|
||||
vect = _board_rotation*vect;
|
||||
|
||||
raw.magnetometer_ga[0] = vect(0);
|
||||
raw.magnetometer_ga[1] = vect(1);
|
||||
raw.magnetometer_ga[2] = vect(2);
|
||||
|
||||
raw.magnetometer_raw[0] = mag_report.x_raw;
|
||||
raw.magnetometer_raw[1] = mag_report.y_raw;
|
||||
|
||||
@@ -37,6 +37,10 @@
|
||||
* Common object definitions without a better home.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @defgroup topics List of all uORB topics.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <drivers/drv_orb_dev.h>
|
||||
|
||||
@@ -55,15 +55,25 @@
|
||||
/* control sets with pre-defined applications */
|
||||
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
|
||||
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
struct actuator_controls_s {
|
||||
uint64_t timestamp;
|
||||
float control[NUM_ACTUATOR_CONTROLS];
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* actuator control sets; this list can be expanded as more controllers emerge */
|
||||
ORB_DECLARE(actuator_controls_0);
|
||||
ORB_DECLARE(actuator_controls_1);
|
||||
ORB_DECLARE(actuator_controls_2);
|
||||
ORB_DECLARE(actuator_controls_3);
|
||||
|
||||
#endif
|
||||
#endif
|
||||
|
||||
@@ -53,11 +53,20 @@
|
||||
#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS
|
||||
#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
struct actuator_controls_effective_s {
|
||||
uint64_t timestamp;
|
||||
float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE];
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* actuator control sets; this list can be expanded as more controllers emerge */
|
||||
ORB_DECLARE(actuator_controls_effective_0);
|
||||
ORB_DECLARE(actuator_controls_effective_1);
|
||||
|
||||
@@ -52,12 +52,21 @@
|
||||
#define NUM_ACTUATOR_OUTPUTS 16
|
||||
#define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
struct actuator_outputs_s {
|
||||
uint64_t timestamp; /**< output timestamp in us since system boot */
|
||||
float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */
|
||||
int noutputs; /**< valid outputs */
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* actuator output sets; this list can be expanded as more drivers emerge */
|
||||
ORB_DECLARE(actuator_outputs_0);
|
||||
ORB_DECLARE(actuator_outputs_1);
|
||||
|
||||
@@ -47,6 +47,7 @@
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
|
||||
@@ -51,10 +51,6 @@
|
||||
#include <stdbool.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* The number of ESCs supported.
|
||||
* Current (Q2/2013) we support 8 ESCs,
|
||||
@@ -76,7 +72,12 @@ enum ESC_CONNECTION_TYPE {
|
||||
};
|
||||
|
||||
/**
|
||||
*
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* Electronic speed controller status.
|
||||
*/
|
||||
struct esc_status_s
|
||||
{
|
||||
|
||||
@@ -46,11 +46,6 @@
|
||||
#include <stdbool.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
enum NAV_CMD {
|
||||
NAV_CMD_WAYPOINT = 0,
|
||||
NAV_CMD_LOITER_TURN_COUNT,
|
||||
@@ -61,6 +56,11 @@ enum NAV_CMD {
|
||||
NAV_CMD_TAKEOFF
|
||||
};
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* Global position setpoint in WGS84 coordinates.
|
||||
*
|
||||
|
||||
@@ -43,11 +43,6 @@
|
||||
#include <stdint.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* Off-board control inputs.
|
||||
*
|
||||
@@ -66,6 +61,11 @@ enum OFFBOARD_CONTROL_MODE
|
||||
OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
|
||||
};
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
struct offboard_control_setpoint_s {
|
||||
uint64_t timestamp;
|
||||
|
||||
|
||||
@@ -46,6 +46,7 @@
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
|
||||
@@ -42,11 +42,20 @@
|
||||
#include <stdint.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
struct parameter_update_s {
|
||||
/** time at which the latest parameter was updated */
|
||||
uint64_t timestamp;
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
ORB_DECLARE(parameter_update);
|
||||
|
||||
#endif
|
||||
@@ -45,11 +45,6 @@
|
||||
#include <stdint.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* The number of RC channel inputs supported.
|
||||
* Current (Q1/2013) radios support up to 18 channels,
|
||||
@@ -85,6 +80,11 @@ enum RC_CHANNELS_FUNCTION
|
||||
RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
|
||||
};
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
struct rc_channels_s {
|
||||
|
||||
uint64_t timestamp; /**< In microseconds since boot time. */
|
||||
|
||||
@@ -46,17 +46,17 @@
|
||||
#include <stdbool.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
enum MAGNETOMETER_MODE {
|
||||
MAGNETOMETER_MODE_NORMAL = 0,
|
||||
MAGNETOMETER_MODE_POSITIVE_BIAS,
|
||||
MAGNETOMETER_MODE_NEGATIVE_BIAS
|
||||
};
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* Sensor readings in raw and SI-unit form.
|
||||
*
|
||||
|
||||
@@ -50,10 +50,6 @@
|
||||
#include <stdbool.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
*/
|
||||
|
||||
enum SUBSYSTEM_TYPE
|
||||
{
|
||||
SUBSYSTEM_TYPE_GYRO = 1,
|
||||
@@ -75,6 +71,10 @@ enum SUBSYSTEM_TYPE
|
||||
SUBSYSTEM_TYPE_RANGEFINDER = 131072
|
||||
};
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
*/
|
||||
|
||||
/**
|
||||
* State of individual sub systems
|
||||
*/
|
||||
|
||||
@@ -50,6 +50,11 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
|
||||
TELEMETRY_STATUS_RADIO_TYPE_WIRE
|
||||
};
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
struct telemetry_status_s {
|
||||
uint64_t timestamp;
|
||||
enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */
|
||||
@@ -62,6 +67,10 @@ struct telemetry_status_s {
|
||||
uint8_t txbuf; /**< how full the tx buffer is as a percentage */
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
ORB_DECLARE(telemetry_status);
|
||||
|
||||
#endif /* TOPIC_TELEMETRY_STATUS_H */
|
||||
@@ -48,6 +48,7 @@
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
|
||||
@@ -45,11 +45,6 @@
|
||||
#include <stdint.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* Commands for commander app.
|
||||
*
|
||||
@@ -110,6 +105,10 @@ enum VEHICLE_CMD_RESULT
|
||||
VEHICLE_CMD_RESULT_ENUM_END=5, /* | */
|
||||
};
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
struct vehicle_command_s
|
||||
{
|
||||
|
||||
@@ -60,8 +60,8 @@
|
||||
*/
|
||||
struct vehicle_global_position_set_triplet_s
|
||||
{
|
||||
bool previous_valid;
|
||||
bool next_valid;
|
||||
bool previous_valid; /**< flag indicating previous position is valid */
|
||||
bool next_valid; /**< flag indicating next position is valid */
|
||||
|
||||
struct vehicle_global_position_setpoint_s previous;
|
||||
struct vehicle_global_position_setpoint_s current;
|
||||
|
||||
@@ -159,6 +159,10 @@ enum VEHICLE_BATTERY_WARNING {
|
||||
VEHICLE_BATTERY_WARNING_ALERT /**< alerting of low voltage 2. stage */
|
||||
};
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* state machine / state of vehicle.
|
||||
|
||||
Reference in New Issue
Block a user