You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
103 lines
3.3 KiB
103 lines
3.3 KiB
/* |
|
AP_RcChannel.cpp - Radio library for Arduino |
|
Code by Jason Short, James Goppert. 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 <math.h> |
|
#include <avr/eeprom.h> |
|
#include "AP_RcChannel.h" |
|
#include "../AP_Common/AP_Common.h" |
|
#include <HardwareSerial.h> |
|
|
|
namespace apo { |
|
|
|
AP_RcChannel::AP_RcChannel(AP_Var::Key keyValue, const prog_char_t * name, |
|
APM_RC_Class * rc, const uint8_t & ch, const uint16_t & pwmMin, |
|
const uint16_t & pwmNeutral, const uint16_t & pwmMax, |
|
const rcMode_t & rcMode, const bool & reverse, const float & scale) : |
|
AP_Var_group(keyValue, name), _ch(this, 1, ch, PSTR("ch")), |
|
_pwmMin(this, 2, pwmMin, PSTR("pMin")), |
|
_pwmNeutral(this, 3, pwmNeutral, PSTR("pNtrl")), |
|
_pwmMax(this, 4, pwmMax, PSTR("pMax")), |
|
_reverse(this, 5, reverse, PSTR("rev")), |
|
_scale(scale == 0 ? AP_Float(0) : AP_Float(this,6,reverse,PSTR("scale"))), |
|
_rcMode(rcMode), _rc(rc), _pwm(pwmNeutral) { |
|
//Serial.print("pwm after ctor: "); Serial.println(pwmNeutral); |
|
if (rcMode == RC_MODE_IN) |
|
return; |
|
//Serial.print("pwm set for ch: "); Serial.println(int(ch)); |
|
rc->OutputCh(ch, pwmNeutral); |
|
|
|
} |
|
|
|
uint16_t AP_RcChannel::getRadioPwm() { |
|
if (_rcMode == RC_MODE_OUT) { |
|
Serial.print("tryng to read from output channel: "); |
|
Serial.println(int(_ch)); |
|
return _pwmNeutral; // if this happens give a safe value of neutral |
|
} |
|
return _rc->InputCh(_ch); |
|
} |
|
|
|
void AP_RcChannel::setPwm(uint16_t pwm) { |
|
//Serial.printf("pwm in setPwm: %d\n", pwm); |
|
//Serial.printf("reverse: %s\n", (reverse)?"true":"false"); |
|
|
|
// apply reverse |
|
if (_reverse) |
|
pwm = int16_t(_pwmNeutral - pwm) + _pwmNeutral; |
|
//Serial.printf("pwm after reverse: %d\n", pwm); |
|
|
|
// apply saturation |
|
if (_pwm > uint8_t(_pwmMax)) |
|
_pwm = _pwmMax; |
|
if (_pwm < uint8_t(_pwmMin)) |
|
_pwm = _pwmMin; |
|
_pwm = pwm; |
|
|
|
//Serial.print("ch: "); Serial.print(ch); Serial.print(" pwm: "); Serial.println(pwm); |
|
if (_rcMode == RC_MODE_IN) |
|
return; |
|
_rc->OutputCh(_ch, _pwm); |
|
} |
|
|
|
uint16_t AP_RcChannel::_positionToPwm(const float & position) { |
|
uint16_t pwm; |
|
//Serial.printf("position: %f\n", position); |
|
if (position < 0) |
|
pwm = position * int16_t(_pwmNeutral - _pwmMin) + _pwmNeutral; |
|
else |
|
pwm = position * int16_t(_pwmMax - _pwmNeutral) + _pwmNeutral; |
|
|
|
if (pwm > uint16_t(_pwmMax)) |
|
pwm = _pwmMax; |
|
if (pwm < uint16_t(_pwmMin)) |
|
pwm = _pwmMin; |
|
return pwm; |
|
} |
|
|
|
float AP_RcChannel::_pwmToPosition(const uint16_t & pwm) { |
|
float position; |
|
// note a piece-wise linear mapping occurs if the pwm ranges |
|
// are not symmetric about pwmNeutral |
|
if (pwm < uint8_t(_pwmNeutral)) |
|
position = 1.0 * int16_t(pwm - _pwmNeutral) / int16_t( |
|
_pwmNeutral - _pwmMin); |
|
else |
|
position = 1.0 * int16_t(pwm - _pwmNeutral) / int16_t( |
|
_pwmMax - _pwmNeutral); |
|
if (position > 1) |
|
position = 1; |
|
if (position < -1) |
|
position = -1; |
|
return position; |
|
} |
|
|
|
} // namespace apo |
|
// vim:ts=4:sw=4:expandtab
|
|
|