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

AP_ServoRelayEvents: More robust check if a channel is available

This commit is contained in:
Michael du Breuil 2018-08-13 17:55:39 -07:00 committed by WickedShell
parent 7fd8211253
commit 588d1898cd
2 changed files with 13 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp
View File

@ -22,13 +22,18 @@
#include "AP_ServoRelayEvents.h"
#include <RC_Channel/RC_Channel.h>
#include <SRV_Channel/SRV_Channel.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
bool AP_ServoRelayEvents::do_set_servo(uint8_t _channel, uint16_t pwm)
{
if (!(mask & 1U<<(_channel-1))) {
// not allowed
SRV_Channel *c = SRV_Channels::srv_channel(_channel-1);
if (c == nullptr) {
return false;
}
if (c->get_function() != SRV_Channel::k_none) {
gcs().send_text(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", _channel);
return false;
}
if (type == EVENT_TYPE_SERVO &&
@ -36,10 +41,6 @@ bool AP_ServoRelayEvents::do_set_servo(uint8_t _channel, uint16_t pwm)
// cancel previous repeat
repeat = 0;
}
SRV_Channel *c = SRV_Channels::srv_channel(_channel-1);
if (c == nullptr) {
return false;
}
c->set_output_pwm(pwm);
return true;
}
@ -67,8 +68,12 @@ bool AP_ServoRelayEvents::do_set_relay(uint8_t relay_num, uint8_t state)
bool AP_ServoRelayEvents::do_repeat_servo(uint8_t _channel, uint16_t _servo_value,
int16_t _repeat, uint16_t _delay_ms)
{
if (!(mask & 1U<<(_channel-1))) {
// not allowed
SRV_Channel *c = SRV_Channels::srv_channel(_channel-1);
if (c == nullptr) {
return false;
}
if (c->get_function() != SRV_Channel::k_none) {
gcs().send_text(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", _channel);
return false;
}
channel = _channel;

4
libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.h
View File

@ -27,9 +27,6 @@ public:
return _singleton;
}
// set allowed servo channel mask
void set_channel_mask(uint16_t _mask) { mask = _mask; }
bool do_set_servo(uint8_t channel, uint16_t pwm);
bool do_set_relay(uint8_t relay_num, uint8_t state);
bool do_repeat_servo(uint8_t channel, uint16_t servo_value, int16_t repeat, uint16_t delay_time_ms);
@ -41,7 +38,6 @@ private:
static AP_ServoRelayEvents *_singleton;
AP_Relay &relay;
uint16_t mask;
// event control state
enum event_type {