Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_IOMCU: implement failsafe PWM

Browse Source
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

44
libraries/AP_IOMCU/AP_IOMCU.cpp
View File

@ -61,6 +61,7 @@ enum iopage {
PAGE_PWM_INFO = 7, PAGE_PWM_INFO = 7,
PAGE_SETUP = 50, PAGE_SETUP = 50,
PAGE_DIRECT_PWM = 54, PAGE_DIRECT_PWM = 54,
PAGE_FAILSAFE_PWM = 55,
PAGE_DISARMED_PWM = 108, PAGE_DISARMED_PWM = 108,
}; };
@ -278,8 +279,17 @@ void AP_IOMCU::thread_main(void)
// update safety pwm // update safety pwm
if (pwm_out.safety_pwm_set != pwm_out.safety_pwm_sent) { if (pwm_out.safety_pwm_set != pwm_out.safety_pwm_sent) {
uint8_t set = pwm_out.safety_pwm_set; 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; 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() 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) { if (pwm_out.num_channels > 0) {
uint8_t n = pwm_out.num_channels; uint8_t n = pwm_out.num_channels;
if (rate.sbus_rate_hz == 0) { if (rate.sbus_rate_hz == 0) {
@ -297,8 +313,9 @@ void AP_IOMCU::send_servo_out()
uint32_t now = AP_HAL::micros(); uint32_t now = AP_HAL::micros();
if (now - last_servo_out_us >= 2000) { if (now - last_servo_out_us >= 2000) {
// don't send data at more than 500Hz // 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; 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 // set mask of channels that ignore safety state
void AP_IOMCU::set_safety_mask(uint16_t chmask) void AP_IOMCU::set_safety_mask(uint16_t chmask)

6
libraries/AP_IOMCU/AP_IOMCU.h
View File

@ -51,6 +51,9 @@ public:
// set mask of channels that ignore safety state // set mask of channels that ignore safety state
void set_safety_mask(uint16_t chmask); 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 enable sbus output
@ -188,6 +191,9 @@ private:
uint8_t safety_pwm_sent; uint8_t safety_pwm_sent;
uint16_t safety_pwm[IOMCU_MAX_CHANNELS]; uint16_t safety_pwm[IOMCU_MAX_CHANNELS];
uint16_t safety_mask; uint16_t safety_mask;
uint16_t failsafe_pwm[IOMCU_MAX_CHANNELS];
uint8_t failsafe_pwm_set;
uint8_t failsafe_pwm_sent;
} pwm_out; } pwm_out;
// read back pwm values // read back pwm values