aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_PiccoloCAN: ESC message rate is now in Hz

Browse Source
This commit is contained in:
Oliver Walters 2020-09-02 13:55:55 +10:00 committed by Andrew Tridgell
parent 5c53c17076
commit 818e7f2cdf
2 changed files with 26 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp
View File

@ -50,14 +50,15 @@ const AP_Param::GroupInfo AP_PiccoloCAN::var_info[] = {
// @Description: Bitmask defining which ESC (motor) channels are to be transmitted over Piccolo CAN // @Description: Bitmask defining which ESC (motor) channels are to be transmitted over Piccolo CAN
// @Bitmask: 0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16 // @Bitmask: 0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16
// @User: Advanced // @User: Advanced
AP_GROUPINFO("ESC_BM", 1, AP_PiccoloCAN, _esc_bm, 0), AP_GROUPINFO("ESC_BM", 1, AP_PiccoloCAN, _esc_bm, 0xFFFF),
// @Param: ESC_MS // @Param: ESC_RT
// @DisplayName: ESC command rate // @DisplayName: ESC output rate
// @Description: Output rate of ESC command messages // @Description: Output rate of ESC command messages
// @Units: ms // @Units: Hz
// @Range: 2 100 // @User: Advanced
AP_GROUPINFO("ESC_MS", 2, AP_PiccoloCAN, _esc_ms, PICCOLO_MSG_RATE_MS_DEFAULT), // @Range: 1 500
AP_GROUPINFO("ESC_RT", 2, AP_PiccoloCAN, _esc_hz, PICCOLO_MSG_RATE_HZ_DEFAULT),
AP_GROUPEND AP_GROUPEND
}; };
@ -134,9 +135,6 @@ void AP_PiccoloCAN::loop()
AP_HAL::CANFrame txFrame; AP_HAL::CANFrame txFrame;
AP_HAL::CANFrame rxFrame; AP_HAL::CANFrame rxFrame;
// How often to transmit CAN messages (milliseconds)
#define CMD_TX_PERIOD 10
uint16_t esc_tx_counter = 0; uint16_t esc_tx_counter = 0;
// CAN Frame ID components // CAN Frame ID components
@ -145,8 +143,21 @@ void AP_PiccoloCAN::loop()
uint64_t timeout; uint64_t timeout;
uint16_t escCmdRateMs;
while (true) { while (true) {
// Calculate the output rate (in ms) based on the configured rate (in Hz)
if (_esc_hz < PICCOLO_MSG_RATE_HZ_MIN) {
_esc_hz = PICCOLO_MSG_RATE_HZ_MIN;
}
if (_esc_hz > PICCOLO_MSG_RATE_HZ_MAX) {
_esc_hz = PICCOLO_MSG_RATE_HZ_MAX;
}
escCmdRateMs = (uint16_t) ((float) 1000 / _esc_hz);
if (!_initialized) { if (!_initialized) {
debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: not initialized\n\r"); debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: not initialized\n\r");
hal.scheduler->delay_microseconds(10000); hal.scheduler->delay_microseconds(10000);
@ -159,13 +170,11 @@ void AP_PiccoloCAN::loop()
hal.scheduler->delay_microseconds(1 * 1000); hal.scheduler->delay_microseconds(1 * 1000);
// Transmit ESC commands at regular intervals // Transmit ESC commands at regular intervals
if (esc_tx_counter++ > _esc_ms) { if (esc_tx_counter++ > escCmdRateMs) {
esc_tx_counter = 0; esc_tx_counter = 0;
if (_esc_ms >= PICCOLO_MSG_RATE_MS_MIN) {
send_esc_messages(); send_esc_messages();
} }
}
// Look for any message responses on the CAN bus // Look for any message responses on the CAN bus
while (read_frame(rxFrame, timeout)) { while (read_frame(rxFrame, timeout)) {

7
libraries/AP_PiccoloCAN/AP_PiccoloCAN.h
View File

@ -35,8 +35,9 @@
#if HAL_PICCOLO_CAN_ENABLE #if HAL_PICCOLO_CAN_ENABLE
#define PICCOLO_MSG_RATE_MS_MIN 2 #define PICCOLO_MSG_RATE_HZ_MIN 1
#define PICCOLO_MSG_RATE_MS_DEFAULT 20 #define PICCOLO_MSG_RATE_HZ_MAX 500
#define PICCOLO_MSG_RATE_HZ_DEFAULT 50
class AP_PiccoloCAN : public AP_CANDriver class AP_PiccoloCAN : public AP_CANDriver
{ {
@ -137,7 +138,7 @@ private:
// Piccolo CAN parameters // Piccolo CAN parameters
AP_Int32 _esc_bm; //! ESC selection bitmask AP_Int32 _esc_bm; //! ESC selection bitmask
AP_Int32 _esc_ms; //! ESC update rate (ms) AP_Int32 _esc_hz; //! ESC update rate (Hz)
}; };