aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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
*/
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
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.
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);
}
}

10
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

@ -109,6 +109,12 @@ public:
serial_setup_output()
*/
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:
struct pwm_group {
@ -232,9 +238,11 @@ private:
const uint16_t dshot_bit_length = 16 + dshot_post;
const uint16_t dshot_buffer_length = dshot_bit_length*4*sizeof(uint32_t);
uint32_t dshot_pulse_time_us;
uint16_t telem_request_mask;
void dma_allocate(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 dshot_send(pwm_group &group, bool blocking);
static void dma_irq_callback(void *p, uint32_t flags);