ardupilot/libraries/APO/AP_RcChannel.cpp

/*
 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