mirror of https://github.com/ArduPilot/ardupilot
104 lines
3.3 KiB
C++
104 lines
3.3 KiB
C++
/*
|
|
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
|