dshot: fix clearing out esc status

This commit is contained in:
Peter van der Perk 2024-03-21 16:06:04 +01:00
parent ea494f5d77
commit eb57942f02
7 changed files with 45 additions and 10 deletions

View File

@ -440,7 +440,7 @@ void up_dshot_trigger(void)
} }
// Calc data now since we're not event driven // Calc data now since we're not event driven
if(bdshot_recv_mask != 0x0) { if (bdshot_recv_mask != 0x0) {
up_bdshot_erpm(); up_bdshot_erpm();
bdshot_recv_mask = 0x0; bdshot_recv_mask = 0x0;
} }

View File

@ -424,15 +424,17 @@ static int timer_set_rate(unsigned timer, unsigned rate)
int channels = get_timer_channels(timer); int channels = get_timer_channels(timer);
irqstate_t flags = px4_enter_critical_section(); irqstate_t flags = px4_enter_critical_section();
for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) { for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) {
if ((1 << channel) & channels) { if ((1 << channel) & channels) {
rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
; ;
rVAL1(channels_timer(channel), timer_io_channels[channel].sub_module) = (BOARD_PWM_FREQ / rate) - 1; rVAL1(channels_timer(channel), timer_io_channels[channel].sub_module) = (BOARD_PWM_FREQ / rate) - 1;
rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
} }
} }
px4_leave_critical_section(flags); px4_leave_critical_section(flags);
return 0; return 0;
} }
@ -442,17 +444,19 @@ static inline void io_timer_set_oneshot_mode(unsigned timer)
int channels = get_timer_channels(timer); int channels = get_timer_channels(timer);
irqstate_t flags = px4_enter_critical_section(); irqstate_t flags = px4_enter_critical_section();
for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) { for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) {
if ((1 << channel) & channels) { if ((1 << channel) & channels) {
uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module); uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module);
rvalue &= ~SMCTRL_PRSC_MASK; rvalue &= ~SMCTRL_PRSC_MASK;
rvalue |= SMCTRL_PRSC_DIV2 | SMCTRL_LDMOD; rvalue |= SMCTRL_PRSC_DIV2 | SMCTRL_LDMOD;
rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
; ;
rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue; rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue;
rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
} }
} }
px4_leave_critical_section(flags); px4_leave_critical_section(flags);
} }
@ -462,17 +466,19 @@ static inline void io_timer_set_PWM_mode(unsigned timer)
int channels = get_timer_channels(timer); int channels = get_timer_channels(timer);
irqstate_t flags = px4_enter_critical_section(); irqstate_t flags = px4_enter_critical_section();
for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) { for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) {
if ((1 << channel) & channels) { if ((1 << channel) & channels) {
uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module); uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module);
rvalue &= ~(SMCTRL_PRSC_MASK | SMCTRL_LDMOD); rvalue &= ~(SMCTRL_PRSC_MASK | SMCTRL_LDMOD);
rvalue |= SMCTRL_PRSC_DIV16; rvalue |= SMCTRL_PRSC_DIV16;
rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
; ;
rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue; rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue;
rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
} }
} }
px4_leave_critical_section(flags); px4_leave_critical_section(flags);
} }
@ -596,7 +602,7 @@ int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode)
rDISMAP0(timer, timer_io_channels[chan].sub_module) = 0xf000; rDISMAP0(timer, timer_io_channels[chan].sub_module) = 0xf000;
rDISMAP1(timer, timer_io_channels[chan].sub_module) = 0xf000; rDISMAP1(timer, timer_io_channels[chan].sub_module) = 0xf000;
rOUTEN(timer) |= timer_io_channels[chan].val_offset == PWMA_VAL ? OUTEN_PWMA_EN(1 << timer_io_channels[chan].sub_module) rOUTEN(timer) |= timer_io_channels[chan].val_offset == PWMA_VAL ? OUTEN_PWMA_EN(1 << timer_io_channels[chan].sub_module)
: OUTEN_PWMB_EN(1 << timer_io_channels[chan].sub_module); : OUTEN_PWMB_EN(1 << timer_io_channels[chan].sub_module);
rDTSRCSEL(timer) = 0; rDTSRCSEL(timer) = 0;
rMCTRL(timer) = MCTRL_LDOK(1 << timer_io_channels[chan].sub_module); rMCTRL(timer) = MCTRL_LDOK(1 << timer_io_channels[chan].sub_module);
rMCTRL(timer) = timer_io_channels[chan].sub_module_bits; rMCTRL(timer) = timer_io_channels[chan].sub_module_bits;

View File

@ -152,6 +152,12 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi
return ret_val == OK ? channels_init_mask : ret_val; return ret_val == OK ? channels_init_mask : ret_val;
} }
int up_bdshot_get_erpm(uint8_t channel, int *erpm)
{
// Not implemented
return -1;
}
void up_bdshot_status(void) void up_bdshot_status(void)
{ {
} }

View File

@ -253,10 +253,6 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem
esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1; esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1;
ret = 1; // Indicate we wrapped, so we publish data ret = 1; // Indicate we wrapped, so we publish data
// reset esc data (in case a motor times out, so we won't send stale data)
memset(&esc_status.esc, 0, sizeof(_telemetry->esc_status_pub.get().esc));
esc_status.esc_online_flags = 0;
} }
_telemetry->last_telemetry_index = telemetry_index; _telemetry->last_telemetry_index = telemetry_index;
@ -264,6 +260,24 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem
return ret; return ret;
} }
void DShot::publish_esc_status(void)
{
esc_status_s &esc_status = _telemetry->esc_status_pub.get();
// clear data of the esc that are offline
for (uint8_t channel = 0; (channel < _telemetry->last_telemetry_index); channel++) {
if ((esc_status.esc_online_flags & (1 << channel)) == 0) {
memset(&esc_status.esc[channel], 0, sizeof(struct esc_report_s));
}
}
// ESC telem wrap around or bdshot update
_telemetry->esc_status_pub.update();
// reset esc online flags
esc_status.esc_online_flags = 0;
}
int DShot::handle_new_bdshot_erpm(void) int DShot::handle_new_bdshot_erpm(void)
{ {
int num_erpms = 0; int num_erpms = 0;
@ -279,9 +293,11 @@ int DShot::handle_new_bdshot_erpm(void)
for (channel = 0; channel < 8; channel++) { for (channel = 0; channel < 8; channel++) {
if (up_bdshot_get_erpm(channel, &erpm) == 0) { if (up_bdshot_get_erpm(channel, &erpm) == 0) {
num_erpms++; num_erpms++;
esc_status.esc_online_flags |= 1 << channel;
esc_status.esc[channel].timestamp = hrt_absolute_time(); esc_status.esc[channel].timestamp = hrt_absolute_time();
esc_status.esc[channel].esc_rpm = (erpm * 100) / esc_status.esc[channel].esc_rpm = (erpm * 100) /
(_param_mot_pole_count.get() / 2); (_param_mot_pole_count.get() / 2);
esc_status.esc[channel].actuator_function = _telemetry->actuator_functions[channel];
} }
} }
@ -521,7 +537,7 @@ void DShot::Run()
if (need_to_publish > 0) { if (need_to_publish > 0) {
// ESC telem wrap around or bdshot update // ESC telem wrap around or bdshot update
_telemetry->esc_status_pub.update(); publish_esc_status();
} }
} }

View File

@ -133,6 +133,8 @@ private:
int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data); int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data);
void publish_esc_status(void);
int handle_new_bdshot_erpm(void); int handle_new_bdshot_erpm(void);
int request_esc_info(); int request_esc_info();

View File

@ -183,6 +183,9 @@ bool DShotTelemetry::decodeByte(uint8_t byte, bool &successful_decoding)
_latest_data.erpm); _latest_data.erpm);
++_num_successful_responses; ++_num_successful_responses;
successful_decoding = true; successful_decoding = true;
} else {
++_num_checksum_errors;
} }
return true; return true;
@ -195,6 +198,7 @@ void DShotTelemetry::printStatus() const
{ {
PX4_INFO("Number of successful ESC frames: %i", _num_successful_responses); PX4_INFO("Number of successful ESC frames: %i", _num_successful_responses);
PX4_INFO("Number of timeouts: %i", _num_timeouts); PX4_INFO("Number of timeouts: %i", _num_timeouts);
PX4_INFO("Number of CRC errors: %i", _num_checksum_errors);
} }
uint8_t DShotTelemetry::updateCrc8(uint8_t crc, uint8_t crc_seed) uint8_t DShotTelemetry::updateCrc8(uint8_t crc, uint8_t crc_seed)

View File

@ -140,4 +140,5 @@ private:
// statistics // statistics
int _num_timeouts{0}; int _num_timeouts{0};
int _num_successful_responses{0}; int _num_successful_responses{0};
int _num_checksum_errors{0};
}; };