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

HAL_SITL: use nan to indicate no actuator value

Browse Source 
this allows for multiple periph nodes, each providing a subset of
motors/servos
This commit is contained in:
Andrew Tridgell 2023-08-21 10:50:37 +10:00 committed by Peter Barker
parent cc9f973007
commit f076f7c4f1
2 changed files with 20 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_HAL_SITL/SITL_Periph_State.cpp
View File

@ -226,9 +226,19 @@ void SimMCast::servo_fd_open(void)
*/
void SimMCast::servo_send(void)
{
const auto *_sitl = AP::sitl();
if (_sitl == nullptr) {
return;
}
uint16_t out[SITL_NUM_CHANNELS] {};
hal.rcout->read(out, SITL_NUM_CHANNELS);
send(servo_fd, (void*)out, sizeof(out), 0);
float out_float[SITL_NUM_CHANNELS];
const uint32_t mask = uint32_t(_sitl->can_servo_mask.get());
for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
out_float[i] = (mask & (1U<<i)) ? out[i] : nanf("");
}
send(servo_fd, (void*)out_float, sizeof(out_float), 0);
}
/*

11
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -692,8 +692,15 @@ void SITL_State::multicast_state_send(void)
void SITL_State::check_servo_input(void)
{
// drain any pending packets
while (recv(servo_in_fd, (void*)mc_servo, sizeof(mc_servo), MSG_DONTWAIT) == sizeof(mc_servo)) {
// noop
float mc_servo_float[SITL_NUM_CHANNELS];
// we loop to ensure we drain all packets from all nodes
while (recv(servo_in_fd, (void*)mc_servo_float, sizeof(mc_servo_float), MSG_DONTWAIT) == sizeof(mc_servo_float)) {
for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
// nan means that node is not outputting this channel
if (!isnan(mc_servo_float[i])) {
mc_servo[i] = uint16_t(mc_servo_float[i]);
}
}
}
}