Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_UAVCAN: added a servo rate limit parameter

Browse Source 
this allows for servos at 50Hz while keeping ESCs at higher rates
This commit is contained in:
Andrew Tridgell 2018-05-24 20:23:00 +10:00
parent afb36ec168
commit b8ae43c30b
2 changed files with 39 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -71,6 +71,14 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("ESC_BM", 3, AP_UAVCAN, _esc_bm, 0), AP_GROUPINFO("ESC_BM", 3, AP_UAVCAN, _esc_bm, 0),
// @Param: SRV_RT
// @DisplayName: Servo output rate
// @Description: Maximum transmit rate for servo outputs
// @Range: 1 200
// @Units: Hz
// @User: Advanced
AP_GROUPINFO("SRV_RT", 4, AP_UAVCAN, _servo_rate_hz, 50),
AP_GROUPEND AP_GROUPEND
}; };
@ -353,7 +361,8 @@ AP_UAVCAN::AP_UAVCAN() :
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) {
_SRV_conf[i].active = false; _SRV_conf[i].esc_pending = false;
_SRV_conf[i].servo_pending = false;
} }
for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) { for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) {
@ -578,7 +587,7 @@ void AP_UAVCAN::SRV_send_servos(void)
* physically possible throws at [-1:1] limits. * physically possible throws at [-1:1] limits.
*/ */
if (_SRV_conf[starting_servo].active && ((((uint32_t) 1) << starting_servo) & _servo_bm)) { if (_SRV_conf[starting_servo].servo_pending && ((((uint32_t) 1) << starting_servo) & _servo_bm)) {
cmd.actuator_id = starting_servo + 1; cmd.actuator_id = starting_servo + 1;
// TODO: other types // TODO: other types
@ -615,7 +624,7 @@ void AP_UAVCAN::SRV_send_esc(void)
for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) {
if ((((uint32_t) 1) << i) & _esc_bm) { if ((((uint32_t) 1) << i) & _esc_bm) {
max_esc_num = i + 1; max_esc_num = i + 1;
if (_SRV_conf[i].active) { if (_SRV_conf[i].esc_pending) {
active_esc_num++; active_esc_num++;
} }
} }
@ -653,31 +662,42 @@ void AP_UAVCAN::do_cyclic(void)
auto *node = get_node(); auto *node = get_node();
const int error = node->spin(uavcan::MonotonicDuration::fromMSec(1)); const int error = node->spin(uavcan::MonotonicDuration::fromUSec(100));
if (error < 0) { if (error < 0) {
hal.scheduler->delay_microseconds(1000); hal.scheduler->delay_microseconds(100);
return; return;
} }
if (SRV_sem_take()) { if (SRV_sem_take()) {
if (_SRV_armed) { if (_SRV_armed) {
bool sent_servos = false;
// if we have any Servos in bitmask
if (_servo_bm > 0) { if (_servo_bm > 0) {
// if we have any Servos in bitmask
uint32_t now = AP_HAL::micros();
const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get());
if (now - _SRV_last_send_us >= servo_period_us) {
_SRV_last_send_us = now;
SRV_send_servos(); SRV_send_servos();
sent_servos = true;
for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) {
_SRV_conf[i].servo_pending = false;
}
}
} }
// if we have any ESC's in bitmask // if we have any ESC's in bitmask
if (_esc_bm > 0) { if (_esc_bm > 0 && !sent_servos) {
SRV_send_esc(); SRV_send_esc();
} }
}
for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) {
// mark as transmitted _SRV_conf[i].esc_pending = false;
_SRV_conf[i].active = false; }
} }
SRV_sem_give(); SRV_sem_give();
@ -786,7 +806,8 @@ void AP_UAVCAN::SRV_arm_actuators(bool arm)
void AP_UAVCAN::SRV_write(uint16_t pulse_len, uint8_t ch) void AP_UAVCAN::SRV_write(uint16_t pulse_len, uint8_t ch)
{ {
_SRV_conf[ch].pulse = pulse_len; _SRV_conf[ch].pulse = pulse_len;
_SRV_conf[ch].active = true; _SRV_conf[ch].esc_pending = true;
_SRV_conf[ch].servo_pending = true;
} }
void AP_UAVCAN::SRV_push_servos() void AP_UAVCAN::SRV_push_servos()

5
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -172,12 +172,14 @@ private:
uint16_t pulse; uint16_t pulse;
uint16_t safety_pulse; uint16_t safety_pulse;
uint16_t failsafe_pulse; uint16_t failsafe_pulse;
bool active; bool esc_pending;
bool servo_pending;
} _SRV_conf[UAVCAN_SRV_NUMBER]; } _SRV_conf[UAVCAN_SRV_NUMBER];
bool _initialized; bool _initialized;
uint8_t _SRV_armed; uint8_t _SRV_armed;
uint8_t _SRV_safety; uint8_t _SRV_safety;
uint32_t _SRV_last_send_us;
typedef struct { typedef struct {
bool enabled; bool enabled;
@ -254,6 +256,7 @@ private:
AP_Int8 _uavcan_node; AP_Int8 _uavcan_node;
AP_Int32 _servo_bm; AP_Int32 _servo_bm;
AP_Int32 _esc_bm; AP_Int32 _esc_bm;
AP_Int16 _servo_rate_hz;
uint8_t _uavcan_i; uint8_t _uavcan_i;