APM_OBC: handle separate terminate pin correctly

This commit is contained in:
Andrew Tridgell 2012-09-07 19:55:51 +10:00
parent 13145e4c01
commit cca04d195a
1 changed files with 3 additions and 2 deletions

View File

@ -166,8 +166,9 @@ APM_OBC::check(APM_OBC::control_mode mode,
break; break;
} }
// if we are not terminating then toggle the heartbeat pin at 10Hz // if we are not terminating or if there is a separate terminate
if (!_terminate && _heartbeat_pin != -1) { // pin configured then toggle the heartbeat pin at 10Hz
if (_heartbeat_pin != -1 && (_terminate_pin != -1 || !_terminate)) {
if (_heartbeat_pin != _last_heartbeat_pin) { if (_heartbeat_pin != _last_heartbeat_pin) {
pinMode(_heartbeat_pin, OUTPUT); pinMode(_heartbeat_pin, OUTPUT);
_last_heartbeat_pin = _heartbeat_pin; _last_heartbeat_pin = _heartbeat_pin;