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

RCOutput_PCA9685: implement gpio methods

This commit is contained in:
Willian Galvani 2022-05-27 16:52:08 -03:00 committed by Andrew Tridgell
parent 018ab14814
commit 603bc5ecce
2 changed files with 36 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp
View File

@ -42,6 +42,9 @@
#define PCA9685_MODE2_OUTNE1_BIT (1 << 1)
#define PCA9685_MODE2_OUTNE0_BIT (1 << 0)
#define PCA9685_LED_ON_H_ALWAYS_ON_BIT (1 << 4)
#define PCA9685_LED_OFF_H_ALWAYS_OFF_BIT (1 << 4)
/*
* Drift for internal oscillator
* see: https://github.com/ArduPilot/ardupilot/commit/50459bdca0b5a1adf95
@ -193,7 +196,9 @@ void RCOutput_PCA9685::write(uint8_t ch, uint16_t period_us)
if (ch >= (PWM_CHAN_COUNT - _channel_offset)) {
return;
}
if (_is_gpio_mask & (1U << ch)) {
return;
}
_pulses_buffer[ch] = period_us;
_pending_write_mask |= (1U << ch);
@ -203,6 +208,21 @@ void RCOutput_PCA9685::write(uint8_t ch, uint16_t period_us)
}
}
void RCOutput_PCA9685::write_gpio(uint8_t chan, bool active)
{
if (chan >= (PWM_CHAN_COUNT - _channel_offset)) {
return;
}
_is_relay_mask |= (1U << ch);
_pulses_buffer[ch] = active;
_pending_write_mask |= (1U << ch);
if (!_corking) {
_corking = true;
push();
}
}
void RCOutput_PCA9685::cork()
{
_corking = true;
@ -240,10 +260,18 @@ void RCOutput_PCA9685::push()
}
uint8_t *d = &pwm_values.data[(ch - min_ch) * 4];
*d++ = 0;
*d++ = 0;
*d++ = length & 0xFF;
*d++ = length >> 8;
if (_is_gpio_mask & (1 << ch)) {
*d++ = 0; // LEDn_ON_L
*d++ = period_us ? PCA9685_LED_ON_H_ALWAYS_ON_BIT : 0; // LEDn_ON_H
*d++ = 0; // LEDn_OFF_L
*d++ = period_us ? 0 : PCA9685_LED_OFF_H_ALWAYS_OFF_BIT; // LEDn_OFF_H
} else {
*d++ = 0; // LEDn_ON_L
*d++ = 0; // LEDn_ON_H
*d++ = length & 0xFF; // LEDn_OFF_L
*d++ = length >> 8; // LEDn_OFF_H
}
}
if (!_dev || !_dev->get_semaphore()->take_nonblocking()) {

3
libraries/AP_HAL_Linux/RCOutput_PCA9685.h
View File

@ -33,6 +33,8 @@ public:
void push() override;
uint16_t read(uint8_t ch) override;
void read(uint16_t* period_us, uint8_t len) override;
bool supports_gpio() override { return true; };
void write_gpio(uint8_t chan, bool active) override;
private:
void reset();
@ -49,6 +51,7 @@ private:
uint8_t _channel_offset;
int16_t _oe_pin_number;
uint32_t _pending_write_mask;
uint32_t _is_gpio_mask;
};
}