TrekkingPhoenix / Mbed 2 deprecated TrekkingControllerV1-4_WinterChallenge20

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed mbed-rtos MotionSensor EthernetInterface

Protocol/receiver.cpp@19:c709c5a9fb08, 2016-05-15 (annotated)

Committer:
drelliak
Date:
Sun May 15 19:14:06 2016 +0000
Revision:
19:c709c5a9fb08
Updated protocol;

Who changed what in which revision?

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