mirror of https://github.com/ArduPilot/ardupilot
51 lines
1.1 KiB
C++
51 lines
1.1 KiB
C++
|
#include "AP_RCProtocol_config.h"
|
||
|
|
|
||
|
|
#if AP_RCPROTOCOL_FDM_ENABLED
|
||
|
|
|
||
|
|
#include "AP_RCProtocol_FDM.h"
|
||
|
|
|
||
|
|
#include <AP_HAL/AP_HAL.h>
|
||
|
|
#include <SITL/SITL.h>
|
||
|
|
|
||
|
|
extern const AP_HAL::HAL& hal;
|
||
|
|
|
||
|
|
void AP_RCProtocol_FDM::update()
|
||
|
|
{
|
||
|
|
const auto sitl = AP::sitl();
|
||
|
|
if (sitl == nullptr) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
const auto &fdm = sitl->state;
|
||
|
|
|
||
|
|
// see if there's fresh input. Until timestamps are worked out,
|
||
|
|
// just check for non-zero values:
|
||
|
|
if (fdm.rcin_chan_count == 0) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// simulate RC input at 50Hz
|
||
|
|
if (AP_HAL::millis() - last_input_ms < 20) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
last_input_ms = AP_HAL::millis();
|
||
|
|
|
||
|
|
// scale from FDM 0-1 floats to PWM values
|
||
|
|
// these are the values that will be fed into the autopilot.
|
||
|
|
uint16_t pwm_input[16];
|
||
|
|
const uint8_t count = MIN(ARRAY_SIZE(pwm_input), fdm.rcin_chan_count);
|
||
|
|
for (uint8_t i=0; i<count; i++) {
|
||
|
|
pwm_input[i] = 1000 + fdm.rcin[i] * 1000;
|
||
|
|
}
|
||
|
|
add_input(
|
||
|
|
count,
|
||
|
|
pwm_input,
|
||
|
|
false, // failsafe
|
||
|
|
0, // check me
|
||
|
|
0 // link quality
|
||
|
|
);
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif // AP_RCPROTOCOL_FDM_ENABLED
|