mirror of https://github.com/ArduPilot/ardupilot
85 lines
1.4 KiB
C++
85 lines
1.4 KiB
C++
/*
|
|
AP_RC.cpp - Radio library for Arduino
|
|
Code by Jason Short. DIYDrones.com
|
|
|
|
This library is free software; you can redistribute it and / or
|
|
modify it under the terms of the GNU Lesser General Public
|
|
License as published by the Free Software Foundation; either
|
|
version 2.1 of the License, or (at your option) any later version.
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include "RC.h"
|
|
|
|
/*
|
|
RC::RC()// :
|
|
_direction_mask(255)
|
|
{
|
|
}
|
|
*/
|
|
|
|
// direction mask: 0 = normal, 1 = reversed servos
|
|
void
|
|
RC::set_channel_direction(uint8_t ch, int8_t dir)
|
|
{
|
|
uint8_t bitflip = 1 << ch;
|
|
|
|
if(dir == 1){
|
|
_direction_mask |= bitflip;
|
|
}else{
|
|
_direction_mask &= ~bitflip;
|
|
}
|
|
}
|
|
|
|
void
|
|
RC::set_failsafe(uint16_t v)
|
|
{
|
|
_fs_value = v;
|
|
}
|
|
|
|
void
|
|
RC::set_mix_mode(uint8_t m)
|
|
{
|
|
_mix_mode = m;
|
|
}
|
|
|
|
|
|
void
|
|
RC::check_throttle_failsafe(uint16_t throttle)
|
|
{
|
|
//check for failsafe and debounce funky reads
|
|
// ------------------------------------------
|
|
if (throttle < _fs_value){
|
|
// we detect a failsafe from radio
|
|
// throttle has dropped below the mark
|
|
_fs_counter++;
|
|
if (_fs_counter == 9){
|
|
|
|
}else if(_fs_counter == 10) {
|
|
failsafe = 1;
|
|
}else if (_fs_counter > 10){
|
|
_fs_counter = 11;
|
|
}
|
|
|
|
}else if(_fs_counter > 0){
|
|
// we are no longer in failsafe condition
|
|
// but we need to recover quickly
|
|
_fs_counter--;
|
|
if (_fs_counter > 3){
|
|
_fs_counter = 3;
|
|
}
|
|
if (_fs_counter == 1){
|
|
|
|
}else if(_fs_counter == 0) {
|
|
failsafe = 0;
|
|
}else if (_fs_counter <0){
|
|
_fs_counter = -1;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|