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

HAL_Linux: RCOutput_PCA9685: implement force_safety_on/off

This commit is contained in:
Willian Galvani 2021-09-08 13:26:36 -03:00
parent 3bf4ebaa4d
commit 1d14ea4f54
2 changed files with 25 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp
View File

@ -167,6 +167,29 @@ void RCOutput_PCA9685::disable_ch(uint8_t ch)
write(ch, 0);
}
bool RCOutput_PCA9685::force_safety_on() {
if (!_dev || !_dev->get_semaphore()->take(10)) {
return false;
}
/* Shutdown before sleeping. */
_dev->write_register(PCA9685_RA_ALL_LED_OFF_H, PCA9685_ALL_LED_OFF_H_SHUT);
/* Put PCA9685 to sleep */
_dev->write_register(PCA9685_RA_MODE1, PCA9685_MODE1_SLEEP_BIT);
_dev->get_semaphore()->give();
return true;
}
void RCOutput_PCA9685::force_safety_off() {
if (!_dev || !_dev->get_semaphore()->take(10)) {
return;
}
/* Restart the device and enable auto-incremented write */
_dev->write_register(PCA9685_RA_MODE1,
PCA9685_MODE1_RESTART_BIT | PCA9685_MODE1_AI_BIT);
_dev->get_semaphore()->give();
}
void RCOutput_PCA9685::write(uint8_t ch, uint16_t period_us)
{
if (ch >= (PWM_CHAN_COUNT - _channel_offset)) {

2
libraries/AP_HAL_Linux/RCOutput_PCA9685.h
View File

@ -26,6 +26,8 @@ public:
uint16_t get_freq(uint8_t ch) override;
void enable_ch(uint8_t ch) override;
void disable_ch(uint8_t ch) override;
bool force_safety_on() override;
void force_safety_off() override;
void write(uint8_t ch, uint16_t period_us) override;
void cork() override;
void push() override;