File content as of revision 17:34fa73413afb:

/*
Copyright 2016 Erik Perillo <erik.perillo@gmail.com>

This file is part of piranha-ptc.

This 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 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. If not, see <http://www.gnu.org/licenses/>.
*/


#include "receiver.h"
#include "EthernetInterface.h"

float Receiver::un_scale(uint16_t value, float min, float max)
{
	return ((float)value)/((1 << 16) - 1)*(max - min) + min;
}

uint8_t Receiver::get_header()
{
	return this->message[MSG_HEADER_IDX];
}

uint16_t Receiver::get_raw_val(int pos)
{
	uint16_t value = 0;

	value |= this->message[MSG_VALS_START_IDX + 2*pos];
	value |= this->message[MSG_VALS_START_IDX + 2*pos + 1] << 8;

	return value;
}

float Receiver::get_val(float min, float max, int pos)
{
	uint16_t raw_val;

	raw_val = this->get_raw_val(pos);
	return this->un_scale(raw_val, min, max);
}

void Receiver::get_vals(float min, float max, float* vals, int size)
{
	uint16_t raw_val;

	for(int i=0; i<size; i++)
	{
		raw_val = this->get_raw_val(i);
		vals[i] = this->un_scale(raw_val, min, max);
	}
}

bool Receiver::receive()
{
	return this->sock.receiveFrom(this->sender_addr, this->message, 
		sizeof(this->message)) > 0;
}

Receiver::Receiver()
{
	;
}

Receiver::Receiver(Endpoint sender_addr, const UDPSocket& sock):
	sock(sock), sender_addr(sender_addr)
{
	;
}

Receiver::Receiver(Endpoint sender_addr, int sock_port, int timeout):
	sender_addr(sender_addr)
{
	this->sock.bind(sock_port);
	this->sock.set_blocking(timeout < 0, timeout);
}

void Receiver::set_sender_addr(const Endpoint& sender_addr)
{
	this->sender_addr = sender_addr;
}

void Receiver::set_socket(const UDPSocket& sock)
{
	this->sock = sock;
}

void Receiver::set_socket(int port, int timeout)
{
	this->sock.bind(port);
	this->sock.set_blocking(timeout < 0, timeout);
}

Endpoint Receiver::get_sender_addr()
{
	return this->sender_addr;
}

UDPSocket Receiver::get_socket()
{
	return this->sock;
}

uint8_t Receiver::get_msg()
{
	return this->message[MSG_HEADER_IDX];
}

float Receiver::get_ang_ref()
{
	return this->get_val(ANG_REF_MIN, ANG_REF_MAX);
}

float Receiver::get_gnd_vel()
{
	return this->get_val(GND_VEL_MIN, GND_VEL_MAX);
}

void Receiver::get_jog_vel(float* period, float* ratio)
{
	*period = this->get_val(JOG_VEL_PERIOD_MIN, JOG_VEL_PERIOD_MAX);
	*ratio = this->get_val(JOG_VEL_RATIO_MIN, JOG_VEL_RATIO_MAX, 1);
}

void Receiver::get_pid_params(float* params)
{
	this->get_vals(PID_PARAMS_MIN, PID_PARAMS_MAX, params, 4);
}

void Receiver::get_mag_calib(float* params)
{
	this->get_vals(MAG_CALIB_MIN, MAG_CALIB_MAX, params, 4);
}