Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_ChibiOS: implement telem request API

Browse Source
This commit is contained in:
Andrew Tridgell 2018-04-02 14:20:58 +10:00
parent 69c623270c
commit 6fd0af11ef
2 changed files with 21 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -776,9 +776,13 @@ void RCOutput::dma_deallocate(Shared_DMA *ctx)
/* /*
create a DSHOT 16 bit packet. Based on prepareDshotPacket from betaflight create a DSHOT 16 bit packet. Based on prepareDshotPacket from betaflight
*/ */
uint16_t RCOutput::create_dshot_packet(const uint16_t value) uint16_t RCOutput::create_dshot_packet(const uint16_t value, bool telem_request)
{ {
uint16_t packet = (value << 1); // no telemetry request uint16_t packet = (value << 1);
if (telem_request) {
packet |= 1;
}
// compute checksum // compute checksum
uint16_t csum = 0; uint16_t csum = 0;
@ -836,7 +840,12 @@ void RCOutput::dshot_send(pwm_group &group, bool blocking)
// dshot values are from 48 to 2047. Zero means off. // dshot values are from 48 to 2047. Zero means off.
value += 47; value += 47;
} }
uint16_t packet = create_dshot_packet(value); uint16_t chan_mask = (1U<<chan);
bool request_telemetry = (telem_request_mask & chan_mask)?true:false;
uint16_t packet = create_dshot_packet(value, request_telemetry);
if (request_telemetry) {
telem_request_mask &= ~chan_mask;
}
fill_DMA_buffer_dshot(group.dma_buffer + i, 4, packet, group.bit_width_mul); fill_DMA_buffer_dshot(group.dma_buffer + i, 4, packet, group.bit_width_mul);
} }
} }

10
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

@ -110,6 +110,12 @@ public:
*/ */
void serial_end(void) override; void serial_end(void) override;
/*
enable telemetry request for a mask of channels. This is used
with DShot to get telemetry feedback
*/
void set_telem_request_mask(uint16_t mask) { telem_request_mask = (mask >> chan_offset); }
private: private:
struct pwm_group { struct pwm_group {
// only advanced timers can do high clocks needed for more than 400Hz // only advanced timers can do high clocks needed for more than 400Hz
@ -232,9 +238,11 @@ private:
const uint16_t dshot_bit_length = 16 + dshot_post; const uint16_t dshot_bit_length = 16 + dshot_post;
const uint16_t dshot_buffer_length = dshot_bit_length*4*sizeof(uint32_t); const uint16_t dshot_buffer_length = dshot_bit_length*4*sizeof(uint32_t);
uint32_t dshot_pulse_time_us; uint32_t dshot_pulse_time_us;
uint16_t telem_request_mask;
void dma_allocate(Shared_DMA *ctx); void dma_allocate(Shared_DMA *ctx);
void dma_deallocate(Shared_DMA *ctx); void dma_deallocate(Shared_DMA *ctx);
uint16_t create_dshot_packet(const uint16_t value); uint16_t create_dshot_packet(const uint16_t value, bool telem_request);
void fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul); void fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul);
void dshot_send(pwm_group &group, bool blocking); void dshot_send(pwm_group &group, bool blocking);
static void dma_irq_callback(void *p, uint32_t flags); static void dma_irq_callback(void *p, uint32_t flags);