TrekkingPhoenix / Mbed 2 deprecated TrekkingControllerV1-4_WinterChallenge20

Ironcup Mar 2020

Dependencies:   mbed mbed-rtos MotionSensor EthernetInterface

Protocol/receiver.cpp@17:34fa73413afb, 2016-05-02 (annotated)

Committer:
drelliak
Date:
Mon May 02 00:58:20 2016 +0000
Revision:
17:34fa73413afb
Added the magnetometer command

Who changed what in which revision?

UserRevisionLine numberNew contents of line
drelliak 17:34fa73413afb 1 /*
drelliak 17:34fa73413afb 2 Copyright 2016 Erik Perillo <erik.perillo@gmail.com>
drelliak 17:34fa73413afb 3
drelliak 17:34fa73413afb 4 This file is part of piranha-ptc.
drelliak 17:34fa73413afb 5
drelliak 17:34fa73413afb 6 This is free software: you can redistribute it and/or modify
drelliak 17:34fa73413afb 7 it under the terms of the GNU General Public License as published by
drelliak 17:34fa73413afb 8 the Free Software Foundation, either version 3 of the License, or
drelliak 17:34fa73413afb 9 (at your option) any later version.
drelliak 17:34fa73413afb 10
drelliak 17:34fa73413afb 11 This is distributed in the hope that it will be useful,
drelliak 17:34fa73413afb 12 but WITHOUT ANY WARRANTY; without even the implied warranty of
drelliak 17:34fa73413afb 13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
drelliak 17:34fa73413afb 14 See the GNU General Public License for more details.
drelliak 17:34fa73413afb 15
drelliak 17:34fa73413afb 16 You should have received a copy of the GNU General Public License
drelliak 17:34fa73413afb 17 along with this. If not, see <http://www.gnu.org/licenses/>.
drelliak 17:34fa73413afb 18 */
drelliak 17:34fa73413afb 19
drelliak 17:34fa73413afb 20
drelliak 17:34fa73413afb 21 #include "receiver.h"
drelliak 17:34fa73413afb 22 #include "EthernetInterface.h"
drelliak 17:34fa73413afb 23
drelliak 17:34fa73413afb 24 float Receiver::un_scale(uint16_t value, float min, float max)
drelliak 17:34fa73413afb 25 {
drelliak 17:34fa73413afb 26 return ((float)value)/((1 << 16) - 1)*(max - min) + min;
drelliak 17:34fa73413afb 27 }
drelliak 17:34fa73413afb 28
drelliak 17:34fa73413afb 29 uint8_t Receiver::get_header()
drelliak 17:34fa73413afb 30 {
drelliak 17:34fa73413afb 31 return this->message[MSG_HEADER_IDX];
drelliak 17:34fa73413afb 32 }
drelliak 17:34fa73413afb 33
drelliak 17:34fa73413afb 34 uint16_t Receiver::get_raw_val(int pos)
drelliak 17:34fa73413afb 35 {
drelliak 17:34fa73413afb 36 uint16_t value = 0;
drelliak 17:34fa73413afb 37
drelliak 17:34fa73413afb 38 value |= this->message[MSG_VALS_START_IDX + 2*pos];
drelliak 17:34fa73413afb 39 value |= this->message[MSG_VALS_START_IDX + 2*pos + 1] << 8;
drelliak 17:34fa73413afb 40
drelliak 17:34fa73413afb 41 return value;
drelliak 17:34fa73413afb 42 }
drelliak 17:34fa73413afb 43
drelliak 17:34fa73413afb 44 float Receiver::get_val(float min, float max, int pos)
drelliak 17:34fa73413afb 45 {
drelliak 17:34fa73413afb 46 uint16_t raw_val;
drelliak 17:34fa73413afb 47
drelliak 17:34fa73413afb 48 raw_val = this->get_raw_val(pos);
drelliak 17:34fa73413afb 49 return this->un_scale(raw_val, min, max);
drelliak 17:34fa73413afb 50 }
drelliak 17:34fa73413afb 51
drelliak 17:34fa73413afb 52 void Receiver::get_vals(float min, float max, float* vals, int size)
drelliak 17:34fa73413afb 53 {
drelliak 17:34fa73413afb 54 uint16_t raw_val;
drelliak 17:34fa73413afb 55
drelliak 17:34fa73413afb 56 for(int i=0; i<size; i++)
drelliak 17:34fa73413afb 57 {
drelliak 17:34fa73413afb 58 raw_val = this->get_raw_val(i);
drelliak 17:34fa73413afb 59 vals[i] = this->un_scale(raw_val, min, max);
drelliak 17:34fa73413afb 60 }
drelliak 17:34fa73413afb 61 }
drelliak 17:34fa73413afb 62
drelliak 17:34fa73413afb 63 bool Receiver::receive()
drelliak 17:34fa73413afb 64 {
drelliak 17:34fa73413afb 65 return this->sock.receiveFrom(this->sender_addr, this->message,
drelliak 17:34fa73413afb 66 sizeof(this->message)) > 0;
drelliak 17:34fa73413afb 67 }
drelliak 17:34fa73413afb 68
drelliak 17:34fa73413afb 69 Receiver::Receiver()
drelliak 17:34fa73413afb 70 {
drelliak 17:34fa73413afb 71 ;
drelliak 17:34fa73413afb 72 }
drelliak 17:34fa73413afb 73
drelliak 17:34fa73413afb 74 Receiver::Receiver(Endpoint sender_addr, const UDPSocket& sock):
drelliak 17:34fa73413afb 75 sock(sock), sender_addr(sender_addr)
drelliak 17:34fa73413afb 76 {
drelliak 17:34fa73413afb 77 ;
drelliak 17:34fa73413afb 78 }
drelliak 17:34fa73413afb 79
drelliak 17:34fa73413afb 80 Receiver::Receiver(Endpoint sender_addr, int sock_port, int timeout):
drelliak 17:34fa73413afb 81 sender_addr(sender_addr)
drelliak 17:34fa73413afb 82 {
drelliak 17:34fa73413afb 83 this->sock.bind(sock_port);
drelliak 17:34fa73413afb 84 this->sock.set_blocking(timeout < 0, timeout);
drelliak 17:34fa73413afb 85 }
drelliak 17:34fa73413afb 86
drelliak 17:34fa73413afb 87 void Receiver::set_sender_addr(const Endpoint& sender_addr)
drelliak 17:34fa73413afb 88 {
drelliak 17:34fa73413afb 89 this->sender_addr = sender_addr;
drelliak 17:34fa73413afb 90 }
drelliak 17:34fa73413afb 91
drelliak 17:34fa73413afb 92 void Receiver::set_socket(const UDPSocket& sock)
drelliak 17:34fa73413afb 93 {
drelliak 17:34fa73413afb 94 this->sock = sock;
drelliak 17:34fa73413afb 95 }
drelliak 17:34fa73413afb 96
drelliak 17:34fa73413afb 97 void Receiver::set_socket(int port, int timeout)
drelliak 17:34fa73413afb 98 {
drelliak 17:34fa73413afb 99 this->sock.bind(port);
drelliak 17:34fa73413afb 100 this->sock.set_blocking(timeout < 0, timeout);
drelliak 17:34fa73413afb 101 }
drelliak 17:34fa73413afb 102
drelliak 17:34fa73413afb 103 Endpoint Receiver::get_sender_addr()
drelliak 17:34fa73413afb 104 {
drelliak 17:34fa73413afb 105 return this->sender_addr;
drelliak 17:34fa73413afb 106 }
drelliak 17:34fa73413afb 107
drelliak 17:34fa73413afb 108 UDPSocket Receiver::get_socket()
drelliak 17:34fa73413afb 109 {
drelliak 17:34fa73413afb 110 return this->sock;
drelliak 17:34fa73413afb 111 }
drelliak 17:34fa73413afb 112
drelliak 17:34fa73413afb 113 uint8_t Receiver::get_msg()
drelliak 17:34fa73413afb 114 {
drelliak 17:34fa73413afb 115 return this->message[MSG_HEADER_IDX];
drelliak 17:34fa73413afb 116 }
drelliak 17:34fa73413afb 117
drelliak 17:34fa73413afb 118 float Receiver::get_ang_ref()
drelliak 17:34fa73413afb 119 {
drelliak 17:34fa73413afb 120 return this->get_val(ANG_REF_MIN, ANG_REF_MAX);
drelliak 17:34fa73413afb 121 }
drelliak 17:34fa73413afb 122
drelliak 17:34fa73413afb 123 float Receiver::get_gnd_vel()
drelliak 17:34fa73413afb 124 {
drelliak 17:34fa73413afb 125 return this->get_val(GND_VEL_MIN, GND_VEL_MAX);
drelliak 17:34fa73413afb 126 }
drelliak 17:34fa73413afb 127
drelliak 17:34fa73413afb 128 void Receiver::get_jog_vel(float* period, float* ratio)
drelliak 17:34fa73413afb 129 {
drelliak 17:34fa73413afb 130 *period = this->get_val(JOG_VEL_PERIOD_MIN, JOG_VEL_PERIOD_MAX);
drelliak 17:34fa73413afb 131 *ratio = this->get_val(JOG_VEL_RATIO_MIN, JOG_VEL_RATIO_MAX, 1);
drelliak 17:34fa73413afb 132 }
drelliak 17:34fa73413afb 133
drelliak 17:34fa73413afb 134 void Receiver::get_pid_params(float* params)
drelliak 17:34fa73413afb 135 {
drelliak 17:34fa73413afb 136 this->get_vals(PID_PARAMS_MIN, PID_PARAMS_MAX, params, 4);
drelliak 17:34fa73413afb 137 }
drelliak 17:34fa73413afb 138
drelliak 17:34fa73413afb 139 void Receiver::get_mag_calib(float* params)
drelliak 17:34fa73413afb 140 {
drelliak 17:34fa73413afb 141 this->get_vals(MAG_CALIB_MIN, MAG_CALIB_MAX, params, 4);
drelliak 17:34fa73413afb 142 }
drelliak 17:34fa73413afb 143