TrekkingPhoenix / Mbed 2 deprecated TrekkingControllerV1-4_WinterChallenge20

Ironcup Mar 2020

Dependencies:   mbed mbed-rtos MotionSensor EthernetInterface

Protocol/receiver.cpp@22:b7cca3089dfe, 2020-09-21 (annotated)

Committer:
starling
Date:
Mon Sep 21 21:45:08 2020 +0000
Revision:
22:b7cca3089dfe
Parent:
21:8a98c6450e00 
01 mar 2020

Who changed what in which revision?

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