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

AP_RobotisServo: send detection commands 4 times

Browse Source
This commit is contained in:
Andrew Tridgell 2018-12-21 18:05:43 +11:00
parent 92f4b37e90
commit 2bff0ece67
2 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_RobotisServo/AP_RobotisServo.cpp
View File

@ -100,6 +100,9 @@ extern const AP_HAL::HAL& hal;
// how many times to send servo configure msgs // how many times to send servo configure msgs
#define CONFIGURE_SERVO_COUNT 4 #define CONFIGURE_SERVO_COUNT 4
// how many times to send servo detection
#define DETECT_SERVO_COUNT 4
const AP_Param::GroupInfo AP_RobotisServo::var_info[] = { const AP_Param::GroupInfo AP_RobotisServo::var_info[] = {
// @Param: POSMIN // @Param: POSMIN
@ -134,7 +137,6 @@ void AP_RobotisServo::init(void)
baudrate = serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Robotis, 0); baudrate = serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Robotis, 0);
us_per_byte = 10 * 1e6 / baudrate; us_per_byte = 10 * 1e6 / baudrate;
us_gap = 4 * 1e6 / baudrate; us_gap = 4 * 1e6 / baudrate;
detect_servos();
} }
} }
@ -280,7 +282,7 @@ void AP_RobotisServo::detect_servos(void)
// give plenty of time for replies from all servos // give plenty of time for replies from all servos
last_send_us = AP_HAL::micros(); last_send_us = AP_HAL::micros();
delay_time_us += 500 * us_per_byte; delay_time_us += 1000 * us_per_byte;
} }
/* /*
@ -411,16 +413,21 @@ void AP_RobotisServo::update()
read_bytes(); read_bytes();
if (servo_mask == 0) {
return;
}
uint32_t now = AP_HAL::micros(); uint32_t now = AP_HAL::micros();
if (last_send_us != 0 && now - last_send_us < delay_time_us) { if (last_send_us != 0 && now - last_send_us < delay_time_us) {
// waiting for last send to complete // waiting for last send to complete
return; return;
} }
if (detection_count < DETECT_SERVO_COUNT) {
detection_count++;
detect_servos();
}
if (servo_mask == 0) {
return;
}
if (configured_servos < CONFIGURE_SERVO_COUNT) { if (configured_servos < CONFIGURE_SERVO_COUNT) {
configured_servos++; configured_servos++;
last_send_us = now; last_send_us = now;

1
libraries/AP_RobotisServo/AP_RobotisServo.h
View File

@ -53,6 +53,7 @@ private:
// auto-detected mask of available servos, from a broadcast ping // auto-detected mask of available servos, from a broadcast ping
uint16_t servo_mask; uint16_t servo_mask;
uint8_t detection_count;
uint8_t configured_servos; uint8_t configured_servos;
bool initialised; bool initialised;