Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_SbusOut: expose SBUS formatting function

This commit is contained in:
Andrew Tridgell 2018-10-29 16:51:22 +11:00
parent b3946fc458
commit e46a640b2c
2 changed files with 62 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
libraries/AP_SBusOut/AP_SBusOut.cpp
View File

@ -73,43 +73,22 @@ AP_SBusOut::AP_SBusOut(void)
AP_Param::setup_object_defaults(this, var_info);
}
/*
* build and send sbus1 frame representing first 16 servo channels
* input arg is pointer to uart
format a SBUS output frame into a 25 byte buffer
*/
void
AP_SBusOut::update()
void AP_SBusOut::sbus_format_frame(uint16_t *channels, uint8_t num_channels, uint8_t buffer[SBUS_BSIZE])
{
if (!initialised) {
initialised = true;
init();
}
if (sbus1_uart == nullptr) {
return;
}
// constrain output rate using sbus_frame_interval
static uint32_t last_micros = 0;
uint32_t now = AP_HAL::micros();
if ((now - last_micros) > sbus_frame_interval) {
last_micros = now;
uint8_t buffer[SBUS_BSIZE] = { 0x0f }; // first byte is always 0x0f
uint8_t index = 1;
uint8_t offset = 0;
uint16_t value;
buffer[0] = 0x0f;
/* construct sbus frame representing channels 1 through 16 (max) */
uint8_t nchan = MIN(NUM_SERVO_CHANNELS, SBUS_CHANNELS);
uint8_t nchan = MIN(num_channels, SBUS_CHANNELS);
for (unsigned i = 0; i < nchan; ++i) {
SRV_Channel *c = SRV_Channels::srv_channel(i);
if (c == nullptr) {
continue;
}
/*protect from out of bounds values and limit to 11 bits*/
uint16_t pwmval = MAX(c->get_output_pwm(), SBUS_MIN);
value = (uint16_t)((pwmval - SBUS_MIN) * SBUS_SCALE);
uint16_t pwmval = MAX(channels[i], SBUS_MIN);
uint16_t value = (uint16_t)((pwmval - SBUS_MIN) * SBUS_SCALE);
if (value > 0x07ff) {
value = 0x07ff;
}
@ -132,6 +111,46 @@ AP_SBusOut::update()
buffer[index + 2] |= (value >> (16 - offset)) & 0xff;
offset += 11;
}
}
/*
* build and send sbus1 frame representing first 16 servo channels
* input arg is pointer to uart
*/
void
AP_SBusOut::update()
{
if (!initialised) {
initialised = true;
init();
}
if (sbus1_uart == nullptr) {
return;
}
// constrain output rate using sbus_frame_interval
static uint32_t last_micros = 0;
uint32_t now = AP_HAL::micros();
if ((now - last_micros) <= sbus_frame_interval) {
return;
}
last_micros = now;
/* construct sbus frame representing channels 1 through 16 (max) */
uint8_t nchan = MIN(NUM_SERVO_CHANNELS, SBUS_CHANNELS);
uint16_t channels[SBUS_CHANNELS] {};
for (unsigned i = 0; i < nchan; ++i) {
SRV_Channel *c = SRV_Channels::srv_channel(i);
if (c == nullptr) {
continue;
}
channels[i] = c->get_output_pwm();
}
uint8_t buffer[SBUS_BSIZE];
sbus_format_frame(channels, nchan, buffer);
#if SBUS_DEBUG
hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
@ -144,8 +163,6 @@ AP_SBusOut::update()
hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
hal.gpio->write(55, 0);
#endif
}
}
void AP_SBusOut::init() {

4
libraries/AP_SBusOut/AP_SBusOut.h
View File

@ -23,7 +23,11 @@ public:
void update();
// public format function for use by IOMCU
static void sbus_format_frame(uint16_t *channels, uint8_t num_channels, uint8_t buffer[25]);
private:
AP_HAL::UARTDriver *sbus1_uart;
void init(void);