ST24 integration in IO firmware

This commit is contained in:
Lorenz Meier 2014-10-07 08:05:45 +02:00
parent cb0cbe479a
commit c2687a7774
4 changed files with 54 additions and 32 deletions

View File

@ -52,7 +52,7 @@
#define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */
static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
static bool dsm_port_input(void);
static bool dsm_port_input(uint8_t *rssi);
static perf_counter_t c_gather_dsm;
static perf_counter_t c_gather_sbus;
@ -60,9 +60,49 @@ static perf_counter_t c_gather_ppm;
static int _dsm_fd;
bool dsm_port_input()
bool dsm_port_input(uint8_t *rssi)
{
perf_begin(c_gather_dsm);
uint16_t temp_count = r_raw_rc_count;
uint8_t n_bytes = 0;
uint8_t *bytes;
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
if (dsm_updated) {
r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
else
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
perf_end(c_gather_dsm);
/* get data from FD and attempt to parse with DSM and ST24 libs */
uint8_t st24_rssi, rx_count;
uint16_t st24_channel_count;
uint8_t st24_maxchans = 18;
bool st24_updated = false;
for (unsigned i = 0; i < n_bytes; i++) {
/* set updated flag if one complete packet was parsed */
st24_updated |= st24_decode(bytes[i], &st24_rssi, &rx_count,
&st24_channel_count, r_raw_rc_values, st24_maxchans);
}
if (st24_updated) {
*rssi = st24_rssi;
r_raw_rc_count = st24_channel_count;
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
return false;
}
@ -127,20 +167,7 @@ controls_tick() {
#endif
perf_begin(c_gather_dsm);
uint16_t temp_count = r_raw_rc_count;
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
if (dsm_updated) {
r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
else
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
(void)dsm_port_input(&rssi);
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
@ -186,18 +213,6 @@ controls_tick() {
}
perf_end(c_gather_ppm);
uint8_t st24_rssi, rx_count;
uint8_t st24_maxchans = 18;
bool st24_updated = st24_decode(0, &st24_rssi, &rx_count, r_raw_rc_values, st24_maxchans);
if (st24_updated) {
rssi = st24_rssi;
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
/* limit number of channels to allowable data size */
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
@ -402,7 +417,7 @@ controls_tick() {
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
/* mix new RC input control values to servos */
if (dsm_updated || sbus_updated || ppm_updated)
if (dsm_updated || sbus_updated || ppm_updated || st24_updated)
mixer_tick();
} else {

View File

@ -452,10 +452,12 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
*
* @param[out] values pointer to per channel array of decoded values
* @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data
* @param[out] n_butes number of bytes read
* @param[out] bytes pointer to the buffer of read bytes
* @return true=decoded raw channel values updated, false=no update
*/
bool
dsm_input(uint16_t *values, uint16_t *num_values)
dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes)
{
ssize_t ret;
hrt_abstime now;
@ -478,8 +480,12 @@ dsm_input(uint16_t *values, uint16_t *num_values)
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)
if (ret < 1) {
return false;
} else {
*n_bytes = ret;
*bytes = &dsm_frame[dsm_partial_frame_count];
}
dsm_last_rx_time = now;

View File

@ -112,6 +112,7 @@
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */
#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */
#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* SBUS input is valid */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */

View File

@ -215,7 +215,7 @@ extern uint16_t adc_measure(unsigned channel);
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 bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes);
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, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);