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

AP_IOMCU: implement failsafe PWM

This commit is contained in:
Andrew Tridgell 2018-09-13 06:22:26 +10:00
parent 3ecc74a59c
commit 098cf8dcb0
2 changed files with 46 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_IOMCU/AP_IOMCU.cpp
View File

@ -61,6 +61,7 @@ enum iopage {
PAGE_PWM_INFO = 7,
PAGE_SETUP = 50,
PAGE_DIRECT_PWM = 54,
PAGE_FAILSAFE_PWM = 55,
PAGE_DISARMED_PWM = 108,
};
@ -278,10 +279,19 @@ void AP_IOMCU::thread_main(void)
// update safety pwm
if (pwm_out.safety_pwm_set != pwm_out.safety_pwm_sent) {
uint8_t set = pwm_out.safety_pwm_set;
write_registers(PAGE_DISARMED_PWM, 0, IOMCU_MAX_CHANNELS, pwm_out.safety_pwm);
if (write_registers(PAGE_DISARMED_PWM, 0, IOMCU_MAX_CHANNELS, pwm_out.safety_pwm)) {
pwm_out.safety_pwm_sent = set;
}
}
// update failsafe pwm
if (pwm_out.failsafe_pwm_set != pwm_out.failsafe_pwm_sent) {
uint8_t set = pwm_out.failsafe_pwm_set;
if (write_registers(PAGE_FAILSAFE_PWM, 0, IOMCU_MAX_CHANNELS, pwm_out.failsafe_pwm)) {
pwm_out.failsafe_pwm_sent = set;
}
}
}
}
/*
@ -289,6 +299,12 @@ void AP_IOMCU::thread_main(void)
*/
void AP_IOMCU::send_servo_out()
{
#if 0
// simple method to test IO failsafe
if (AP_HAL::millis() > 30000) {
return;
}
#endif
if (pwm_out.num_channels > 0) {
uint8_t n = pwm_out.num_channels;
if (rate.sbus_rate_hz == 0) {
@ -297,10 +313,11 @@ void AP_IOMCU::send_servo_out()
uint32_t now = AP_HAL::micros();
if (now - last_servo_out_us >= 2000) {
// don't send data at more than 500Hz
write_registers(PAGE_DIRECT_PWM, 0, n, pwm_out.pwm);
if (write_registers(PAGE_DIRECT_PWM, 0, n, pwm_out.pwm)) {
last_servo_out_us = now;
}
}
}
}
/*
@ -729,6 +746,25 @@ void AP_IOMCU::set_safety_pwm(uint16_t chmask, uint16_t period_us)
}
}
/*
set the pwm to use when in FMU failsafe
*/
void AP_IOMCU::set_failsafe_pwm(uint16_t chmask, uint16_t period_us)
{
bool changed = false;
for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) {
if (chmask & (1U<<i)) {
if (pwm_out.failsafe_pwm[i] != period_us) {
pwm_out.failsafe_pwm[i] = period_us;
changed = true;
}
}
}
if (changed) {
pwm_out.failsafe_pwm_set++;
}
}
// set mask of channels that ignore safety state
void AP_IOMCU::set_safety_mask(uint16_t chmask)

6
libraries/AP_IOMCU/AP_IOMCU.h
View File

@ -52,6 +52,9 @@ public:
// set mask of channels that ignore safety state
void set_safety_mask(uint16_t chmask);
// set PWM of channels when in FMU failsafe
void set_failsafe_pwm(uint16_t chmask, uint16_t period_us);
/*
enable sbus output
*/
@ -188,6 +191,9 @@ private:
uint8_t safety_pwm_sent;
uint16_t safety_pwm[IOMCU_MAX_CHANNELS];
uint16_t safety_mask;
uint16_t failsafe_pwm[IOMCU_MAX_CHANNELS];
uint8_t failsafe_pwm_set;
uint8_t failsafe_pwm_sent;
} pwm_out;
// read back pwm values