forked from Archive/PX4-Autopilot
ST24 integration in IO firmware
This commit is contained in:
parent
cb0cbe479a
commit
c2687a7774
|
@ -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 {
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue