ardupilot/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp

130 lines
3.4 KiB
C++

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* AP_ServoRelayEvents - handle servo and relay MAVLink events
*/
#include <AP_HAL.h>
#include <AP_Common.h>
#include <AP_ServoRelayEvents.h>
#include <RC_Channel.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
return false;
}
if (type == EVENT_TYPE_SERVO &&
channel == _channel) {
// cancel previous repeat
repeat = 0;
}
hal.rcout->enable_ch(_channel-1);
hal.rcout->write(_channel-1, pwm);
return true;
}
bool AP_ServoRelayEvents::do_set_relay(uint8_t relay_num, uint8_t state)
{
if (!relay.enabled(relay_num)) {
return false;
}
if (type == EVENT_TYPE_RELAY &&
channel == relay_num) {
// cancel previous repeat
repeat = 0;
}
if (state == 1) {
relay.on(relay_num);
} else if (state == 0) {
relay.off(relay_num);
} else {
relay.toggle(relay_num);
}
return true;
}
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
return false;
}
channel = _channel;
type = EVENT_TYPE_SERVO;
start_time_ms = 0;
delay_ms = _delay_ms / 2;
repeat = _repeat * 2;
servo_value = _servo_value;
update_events();
return true;
}
bool AP_ServoRelayEvents::do_repeat_relay(uint8_t relay_num, int16_t _repeat, uint32_t _delay_ms)
{
if (!relay.enabled(relay_num)) {
return false;
}
type = EVENT_TYPE_RELAY;
channel = relay_num;
start_time_ms = 0;
delay_ms = _delay_ms/2; // half cycle time
repeat = _repeat*2; // number of full cycles
update_events();
return true;
}
/*
update state for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
*/
void AP_ServoRelayEvents::update_events(void)
{
if (repeat == 0 || (hal.scheduler->millis() - start_time_ms) < delay_ms) {
return;
}
start_time_ms = hal.scheduler->millis();
switch (type) {
case EVENT_TYPE_SERVO:
hal.rcout->enable_ch(channel-1);
if (repeat & 1) {
hal.rcout->write(channel-1, RC_Channel::rc_channel(channel-1)->radio_trim);
} else {
hal.rcout->write(channel-1, servo_value);
}
break;
case EVENT_TYPE_RELAY:
relay.toggle(channel);
break;
}
if (repeat > 0) {
repeat--;
} else {
// toggle bottom bit so servos flip in value
repeat ^= 1;
}
}