|
|
@ -175,12 +175,12 @@ void AP_PiccoloCAN::loop() |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
// Calculate the output rate for ESC commands
|
|
|
|
// Calculate the output rate for ESC commands
|
|
|
|
_esc_hz = constrain_int16(_esc_hz, PICCOLO_MSG_RATE_HZ_MIN, PICCOLO_MSG_RATE_HZ_MAX); |
|
|
|
_esc_hz.set(constrain_int16(_esc_hz, PICCOLO_MSG_RATE_HZ_MIN, PICCOLO_MSG_RATE_HZ_MAX)); |
|
|
|
|
|
|
|
|
|
|
|
uint16_t escCmdRateMs = 1000 / _esc_hz; |
|
|
|
uint16_t escCmdRateMs = 1000 / _esc_hz; |
|
|
|
|
|
|
|
|
|
|
|
// Calculate the output rate for servo commands
|
|
|
|
// Calculate the output rate for servo commands
|
|
|
|
_srv_hz = constrain_int16(_srv_hz, PICCOLO_MSG_RATE_HZ_MIN, PICCOLO_MSG_RATE_HZ_MAX); |
|
|
|
_srv_hz.set(constrain_int16(_srv_hz, PICCOLO_MSG_RATE_HZ_MIN, PICCOLO_MSG_RATE_HZ_MAX)); |
|
|
|
|
|
|
|
|
|
|
|
uint16_t servoCmdRateMs = 1000 / _srv_hz; |
|
|
|
uint16_t servoCmdRateMs = 1000 / _srv_hz; |
|
|
|
|
|
|
|
|
|
|
|