Ruprecht Altenburger / Mbed OS mirror_actuator_V1

Mirror actuator for RT2 lab

Dependencies:   FastPWM

uart_comm_thread.cpp@11:d43f8b421d6d, 2021-04-28 (annotated)

Committer:
altb2
Date:
Wed Apr 28 12:51:02 2021 +0000
Revision:
11:d43f8b421d6d
Parent:
8:49ac75c42da0
Child:
15:9f32f64eee5b
interm2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 7:942fd77d5e19 1 // includes
altb2 7:942fd77d5e19 2 #include "uart_comm_thread.h"
altb2 7:942fd77d5e19 3 /*
altb2 7:942fd77d5e19 4 -------- DATA PROTOCOL----------------------------
altb2 7:942fd77d5e19 5 254 1 255 201 1 4 0 ...
altb2 7:942fd77d5e19 6 n1 n2 rec id1 id2 #Byte1 #Byte2 thedata
altb2 7:942fd77d5e19 7 --------------------------------------------------
altb2 7:942fd77d5e19 8 1-20 sensor values,
altb2 7:942fd77d5e19 9 id1 id2
altb2 7:942fd77d5e19 10 10 Counter values
altb2 7:942fd77d5e19 11 1 c1
altb2 7:942fd77d5e19 12 2 c2 (increments)
altb2 7:942fd77d5e19 13 11 actual current
altb2 7:942fd77d5e19 14 1 i1 / A
altb2 7:942fd77d5e19 15 2 i2 / A
altb2 7:942fd77d5e19 16 --------------------------------------------------
altb2 7:942fd77d5e19 17 // NOT USED: 21-40 cntrl values, 21 desired values 1 Phi1 / rad 2 Phi2 / rad 3 x / mm 4 y / mm
altb2 7:942fd77d5e19 18 --------------------------------------------------
altb2 7:942fd77d5e19 19 101-120 estimates/actual values,
altb2 7:942fd77d5e19 20 101 angles and calculated x,y
altb2 7:942fd77d5e19 21 1 Phi1 / rad
altb2 7:942fd77d5e19 22 2 Phi2 / rad
altb2 7:942fd77d5e19 23 3 x / mm
altb2 7:942fd77d5e19 24 4 y / mm
altb2 7:942fd77d5e19 25
altb2 7:942fd77d5e19 26 --------------------------------------------------
altb2 7:942fd77d5e19 27 121-140 send techn. values, like offsets
altb2 7:942fd77d5e19 28 id1 id2
altb2 7:942fd77d5e19 29 121
altb2 7:942fd77d5e19 30 1 inc_offset phi1 / increments int16_t
altb2 7:942fd77d5e19 31 2 inc_offset phi2 / " "
altb2 7:942fd77d5e19 32 3 inc_additional_offset phi1 / increments int16_t
altb2 7:942fd77d5e19 33 4 inc_additional_offset phi2 / " "
altb2 7:942fd77d5e19 34 125
altb2 7:942fd77d5e19 35 1 num_it of X2P trafo
altb2 7:942fd77d5e19 36 --------------------------------------------------
altb2 7:942fd77d5e19 37
altb2 7:942fd77d5e19 38 2xx: set-value and commands (like disable controller...)
altb2 7:942fd77d5e19 39 id1 id2
altb2 7:942fd77d5e19 40 201: Set values/parameters
altb2 7:942fd77d5e19 41 1 inc_additional_offset phi1 / increments int16_t
altb2 7:942fd77d5e19 42 2 inc_additional_offset phi2 / " "
altb2 7:942fd77d5e19 43
altb2 7:942fd77d5e19 44 202: set desired absolute values
altb2 7:942fd77d5e19 45 1 phi1 rad float
altb2 7:942fd77d5e19 46 2 phi2 rad float
altb2 7:942fd77d5e19 47 3 x mm float
altb2 7:942fd77d5e19 48 4 y mm float
altb2 7:942fd77d5e19 49 203 Increment values
altb2 7:942fd77d5e19 50 1 dphi1 rad float
altb2 7:942fd77d5e19 51 2 dphi2 rad float
altb2 7:942fd77d5e19 52 3 dx mm float
altb2 7:942fd77d5e19 53 4 dy mm float
altb2 7:942fd77d5e19 54 220 Laser on/off
altb2 7:942fd77d5e19 55 1 0 = off, 1 = on
altb2 7:942fd77d5e19 56 221 Trafo on/off
altb2 7:942fd77d5e19 57 1 0 = off, 1 = on
altb2 7:942fd77d5e19 58 230 external control on/off
altb2 7:942fd77d5e19 59 1 0 = off, 1 = on
altb2 8:49ac75c42da0 60 241
altb2 8:49ac75c42da0 61 1 Send text
altb2 8:49ac75c42da0 62 250
altb2 8:49ac75c42da0 63 1 GPA message
altb2 7:942fd77d5e19 64
altb2 7:942fd77d5e19 65 */
altb2 7:942fd77d5e19 66
altb2 8:49ac75c42da0 67
altb2 8:49ac75c42da0 68 extern GPA myGPA;
altb2 8:49ac75c42da0 69
altb2 7:942fd77d5e19 70 // #### constructor
altb2 7:942fd77d5e19 71 uart_comm_thread::uart_comm_thread(BufferedSerial *com, float Ts): thread(osPriorityBelowNormal, 4096)
altb2 7:942fd77d5e19 72 {
altb2 7:942fd77d5e19 73 // init serial
altb2 7:942fd77d5e19 74 uart = com;
altb2 7:942fd77d5e19 75 //uart->attach(callback(this, &uart_comm_thread::callBack), RawSerial::RxIrq);
altb2 7:942fd77d5e19 76 this->Ts = Ts;
altb2 8:49ac75c42da0 77 gpa_stop_sent = false;
altb2 7:942fd77d5e19 78 init();
altb2 7:942fd77d5e19 79 }
altb2 7:942fd77d5e19 80
altb2 7:942fd77d5e19 81 // #### destructor
altb2 7:942fd77d5e19 82 uart_comm_thread::~uart_comm_thread() {
altb2 7:942fd77d5e19 83 }
altb2 7:942fd77d5e19 84
altb2 7:942fd77d5e19 85 // #### request data from device
altb2 7:942fd77d5e19 86 void uart_comm_thread::init(){
altb2 7:942fd77d5e19 87 // init statemachine
altb2 7:942fd77d5e19 88 }
altb2 7:942fd77d5e19 89
altb2 7:942fd77d5e19 90
altb2 7:942fd77d5e19 91 // #### run the statemachine
altb2 7:942fd77d5e19 92 void uart_comm_thread::run(void)
altb2 7:942fd77d5e19 93 {
altb2 7:942fd77d5e19 94 // returnvalue
altb2 7:942fd77d5e19 95 bool retVal = false;
altb2 7:942fd77d5e19 96 uint8_t checksum,k;
altb2 7:942fd77d5e19 97 uint8_t send_state =101;
altb2 7:942fd77d5e19 98 while(true)
altb2 7:942fd77d5e19 99 {
altb2 7:942fd77d5e19 100 ThisThread::flags_wait_any(threadFlag);
altb2 7:942fd77d5e19 101 //--- The LOOP --------------------------------------------------------
altb2 7:942fd77d5e19 102 uint32_t num = uart->read(buffer, sizeof(buffer));
altb2 7:942fd77d5e19 103 if (num >0)
altb2 7:942fd77d5e19 104 {
altb2 7:942fd77d5e19 105 if(buffer[0] == 254 && buffer[1] == 1)
altb2 7:942fd77d5e19 106 {
altb2 7:942fd77d5e19 107 if(analyse_received_data())
altb2 7:942fd77d5e19 108 ;// led1 = !led1;
altb2 7:942fd77d5e19 109 }
altb2 7:942fd77d5e19 110 }
altb2 7:942fd77d5e19 111 switch(send_state)
altb2 7:942fd77d5e19 112 {
altb2 7:942fd77d5e19 113 case 101:
altb2 11:d43f8b421d6d 114 send_f_data(101,12,2*4,data.sens_phi); // send actual phi values (1 and 2)
altb2 11:d43f8b421d6d 115 send_f_data(101,34,2*4,data.est_xy); // send actual xy values
altb2 11:d43f8b421d6d 116 send_state = 202;
altb2 7:942fd77d5e19 117 break;
altb2 11:d43f8b421d6d 118 case 121: // currently omitted
altb2 11:d43f8b421d6d 119 send_data(121,34,2*2,mk.inc_additional_offset);
altb2 7:942fd77d5e19 120 send_state = 202;
altb2 7:942fd77d5e19 121 break;
altb2 11:d43f8b421d6d 122 case 202: // desired phi
altb2 11:d43f8b421d6d 123 send_f_data(202,12,2*4,data.cntrl_phi_des);
altb2 8:49ac75c42da0 124 send_state = 250;
altb2 8:49ac75c42da0 125 break;
altb2 11:d43f8b421d6d 126 case 125: // number of iterations in the trafo
altb2 11:d43f8b421d6d 127 send_data(125,1,data.num_it);
altb2 11:d43f8b421d6d 128 send_state = 250;
altb2 11:d43f8b421d6d 129 break;
altb2 11:d43f8b421d6d 130 case 250: // send GPA values
altb2 8:49ac75c42da0 131 if(myGPA.new_data_available)
altb2 8:49ac75c42da0 132 {
altb2 8:49ac75c42da0 133 float dum[8];
altb2 8:49ac75c42da0 134 myGPA.getGPAdata(dum);
altb2 11:d43f8b421d6d 135 send_f_data(250,1,(uint16_t)32,dum); // send new values (8 floats)
altb2 8:49ac75c42da0 136 }
altb2 8:49ac75c42da0 137 else if(myGPA.start_now)
altb2 8:49ac75c42da0 138 {
altb2 11:d43f8b421d6d 139 send_char_data(250,2,0); // send start flag
altb2 8:49ac75c42da0 140 myGPA.start_now = false;
altb2 8:49ac75c42da0 141 gpa_stop_sent = false;
altb2 8:49ac75c42da0 142 }
altb2 8:49ac75c42da0 143 else if(myGPA.meas_is_finished && !gpa_stop_sent && !myGPA.new_data_available)
altb2 8:49ac75c42da0 144 {
altb2 11:d43f8b421d6d 145 send_char_data(250,255,1); // send stop flag
altb2 8:49ac75c42da0 146 gpa_stop_sent = true;
altb2 8:49ac75c42da0 147 }
altb2 7:942fd77d5e19 148 send_state = 101;
altb2 8:49ac75c42da0 149 break;
altb2 7:942fd77d5e19 150 default:
altb2 7:942fd77d5e19 151 break;
altb2 7:942fd77d5e19 152 }
altb2 7:942fd77d5e19 153 }// loop
altb2 7:942fd77d5e19 154 }
altb2 7:942fd77d5e19 155
altb2 7:942fd77d5e19 156
altb2 7:942fd77d5e19 157
altb2 7:942fd77d5e19 158 // #### receive data from hardware buffer, ensuring no dropped bytes
altb2 7:942fd77d5e19 159
altb2 7:942fd77d5e19 160 // ------------------- start uart ----------------
altb2 7:942fd77d5e19 161 void uart_comm_thread::start_uart(void){
altb2 7:942fd77d5e19 162
altb2 7:942fd77d5e19 163 thread.start(callback(this, &uart_comm_thread::run));
altb2 7:942fd77d5e19 164 ticker.attach(callback(this, &uart_comm_thread::sendThreadFlag), Ts);
altb2 7:942fd77d5e19 165 }
altb2 7:942fd77d5e19 166 // this is for realtime OS
altb2 7:942fd77d5e19 167 void uart_comm_thread::sendThreadFlag() {
altb2 7:942fd77d5e19 168 thread.flags_set(threadFlag);
altb2 7:942fd77d5e19 169 }
altb2 7:942fd77d5e19 170
altb2 8:49ac75c42da0 171 void uart_comm_thread::send_f_data(char id1,char id2,uint16_t N,float *dat)
altb2 7:942fd77d5e19 172 {
altb2 7:942fd77d5e19 173 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 7:942fd77d5e19 174 buffer[3] = id1;
altb2 7:942fd77d5e19 175 buffer[4] = id2;
altb2 8:49ac75c42da0 176 buffer[5] = N%256;
altb2 8:49ac75c42da0 177 buffer[6] = N/256;
altb2 7:942fd77d5e19 178 uart->write(buffer, 7);
altb2 7:942fd77d5e19 179 char *float_data = (char *)dat;
altb2 7:942fd77d5e19 180 uart->write(float_data,N);
altb2 7:942fd77d5e19 181 uart->write("\r",2); // line end
altb2 7:942fd77d5e19 182 }
altb2 11:d43f8b421d6d 183 // send uint16 data
altb2 7:942fd77d5e19 184 void uart_comm_thread::send_data(char id1,char id2,uint16_t N,int16_t *dat)
altb2 7:942fd77d5e19 185 {
altb2 7:942fd77d5e19 186 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 7:942fd77d5e19 187 buffer[3] = id1;
altb2 7:942fd77d5e19 188 buffer[4] = id2;
altb2 7:942fd77d5e19 189 buffer[5] = *(char *)N;
altb2 7:942fd77d5e19 190 uart->write(buffer, 7);
altb2 7:942fd77d5e19 191 char *int_data = (char *)dat;
altb2 7:942fd77d5e19 192 uart->write(int_data,N);
altb2 7:942fd77d5e19 193 uart->write("\r",2); // line end
altb2 7:942fd77d5e19 194 }
altb2 11:d43f8b421d6d 195 // send one uint16 data
altb2 7:942fd77d5e19 196 void uart_comm_thread::send_data(char id1,char id2,int16_t dat)
altb2 7:942fd77d5e19 197 {
altb2 7:942fd77d5e19 198 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 7:942fd77d5e19 199 buffer[3] = id1;
altb2 7:942fd77d5e19 200 buffer[4] = id2;
altb2 7:942fd77d5e19 201 buffer[5] = 2;
altb2 7:942fd77d5e19 202 buffer[6] = 0;
altb2 7:942fd77d5e19 203 uart->write(buffer, 7);
altb2 7:942fd77d5e19 204 char *int_data = (char *)dat;
altb2 7:942fd77d5e19 205 uart->write(int_data,2);
altb2 7:942fd77d5e19 206 uart->write("\r",2); // line end
altb2 7:942fd77d5e19 207 }
altb2 11:d43f8b421d6d 208 // send text
altb2 8:49ac75c42da0 209 void uart_comm_thread::send_char_data(char id1,char id2,uint8_t dat)
altb2 7:942fd77d5e19 210 {
altb2 7:942fd77d5e19 211 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 7:942fd77d5e19 212 buffer[3] = id1;
altb2 7:942fd77d5e19 213 buffer[4] = id2;
altb2 7:942fd77d5e19 214 buffer[5] = 1;
altb2 7:942fd77d5e19 215 buffer[6] = 0;
altb2 7:942fd77d5e19 216 uart->write(buffer, 7);
altb2 7:942fd77d5e19 217 char int8_data = (char)dat;
altb2 7:942fd77d5e19 218 uart->write(&int8_data,1);
altb2 7:942fd77d5e19 219 uart->write("\r",2); // line end
altb2 7:942fd77d5e19 220 }
altb2 8:49ac75c42da0 221 void uart_comm_thread::send_text(const char *txt)
altb2 8:49ac75c42da0 222 {
altb2 8:49ac75c42da0 223 uint16_t N=0;
altb2 11:d43f8b421d6d 224 while(txt[N] != '\0') // get length of text
altb2 8:49ac75c42da0 225 N++;
altb2 8:49ac75c42da0 226 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 8:49ac75c42da0 227 buffer[3] = 241;
altb2 8:49ac75c42da0 228 buffer[4] = 1;
altb2 8:49ac75c42da0 229 buffer[5] = *(char *)N;
altb2 8:49ac75c42da0 230 uart->write(buffer, 7);
altb2 8:49ac75c42da0 231 uart->write(txt,N);
altb2 8:49ac75c42da0 232 uart->write("\r",2); // line end
altb2 8:49ac75c42da0 233 }
altb2 11:d43f8b421d6d 234 // -----------------------------------------------------------------------------
altb2 11:d43f8b421d6d 235 // analyse data, see comments at top of this file for numbering
altb2 7:942fd77d5e19 236 bool uart_comm_thread::analyse_received_data(void){
altb2 7:942fd77d5e19 237 char msg_id1 = buffer[3];
altb2 7:942fd77d5e19 238 char msg_id2 = buffer[4];
altb2 7:942fd77d5e19 239 uint16_t N = 256 * buffer[6] + buffer[5];
altb2 7:942fd77d5e19 240 switch(msg_id1)
altb2 7:942fd77d5e19 241 {
altb2 11:d43f8b421d6d 242 case 201: // add additional offsets to offsets
altb2 7:942fd77d5e19 243 if(N != 2)
altb2 7:942fd77d5e19 244 return false;
altb2 7:942fd77d5e19 245 switch(msg_id2)
altb2 7:942fd77d5e19 246 {
altb2 11:d43f8b421d6d 247 case 1: // for phi1
altb2 7:942fd77d5e19 248 mk.add_additional_offsets(256 * buffer[8] + buffer[7],0);
altb2 7:942fd77d5e19 249 return true;
altb2 7:942fd77d5e19 250 break;
altb2 11:d43f8b421d6d 251 case 2: // for phi2
altb2 7:942fd77d5e19 252 mk.add_additional_offsets(0,256 * buffer[8] + buffer[7]);
altb2 7:942fd77d5e19 253 return true;
altb2 7:942fd77d5e19 254 break;
altb2 7:942fd77d5e19 255 default:
altb2 7:942fd77d5e19 256 break;
altb2 7:942fd77d5e19 257 }
altb2 7:942fd77d5e19 258 break; // case 201
altb2 11:d43f8b421d6d 259 case 202: // set desired phi or xy-values
altb2 7:942fd77d5e19 260 if(N != 4)
altb2 7:942fd77d5e19 261 return false;
altb2 7:942fd77d5e19 262 switch(msg_id2)
altb2 7:942fd77d5e19 263 {
altb2 7:942fd77d5e19 264 case 1:
altb2 7:942fd77d5e19 265 data.cntrl_phi_des[0] = *(float *)&buffer[7];
altb2 7:942fd77d5e19 266 return true;
altb2 7:942fd77d5e19 267 break;
altb2 7:942fd77d5e19 268 case 2:
altb2 7:942fd77d5e19 269 data.cntrl_phi_des[1] = *(float *)&buffer[7];
altb2 7:942fd77d5e19 270 return true;
altb2 7:942fd77d5e19 271 break;
altb2 7:942fd77d5e19 272 case 3:
altb2 11:d43f8b421d6d 273 data.cntrl_xy_des[0] = *(float *)&buffer[7];
altb2 7:942fd77d5e19 274 return true;
altb2 7:942fd77d5e19 275 break;
altb2 7:942fd77d5e19 276 case 4:
altb2 7:942fd77d5e19 277 data.cntrl_xy_des[1] = *(float *)&buffer[7];
altb2 7:942fd77d5e19 278 return true;
altb2 7:942fd77d5e19 279 break;
altb2 7:942fd77d5e19 280 default:
altb2 7:942fd77d5e19 281 break;
altb2 7:942fd77d5e19 282 }
altb2 7:942fd77d5e19 283 break; // case 202
altb2 11:d43f8b421d6d 284 case 203: // increment desired values
altb2 7:942fd77d5e19 285 if(N != 4)
altb2 7:942fd77d5e19 286 return false;
altb2 7:942fd77d5e19 287 switch(msg_id2)
altb2 7:942fd77d5e19 288 {
altb2 7:942fd77d5e19 289 case 1:
altb2 7:942fd77d5e19 290 data.cntrl_phi_des[0] += *(float *)&buffer[7];
altb2 7:942fd77d5e19 291 return true;
altb2 7:942fd77d5e19 292 break;
altb2 7:942fd77d5e19 293 case 2:
altb2 7:942fd77d5e19 294 data.cntrl_phi_des[1] += *(float *)&buffer[7];
altb2 7:942fd77d5e19 295 return true;
altb2 7:942fd77d5e19 296 break;
altb2 7:942fd77d5e19 297 case 3:
altb2 11:d43f8b421d6d 298 data.cntrl_xy_des[0] += *(float *)&buffer[7];
altb2 7:942fd77d5e19 299 return true;
altb2 7:942fd77d5e19 300 break;
altb2 7:942fd77d5e19 301 case 4:
altb2 7:942fd77d5e19 302 data.cntrl_xy_des[1] += *(float *)&buffer[7];
altb2 7:942fd77d5e19 303 return true;
altb2 7:942fd77d5e19 304 break;
altb2 7:942fd77d5e19 305 default:
altb2 7:942fd77d5e19 306 break;
altb2 7:942fd77d5e19 307 }
altb2 7:942fd77d5e19 308 break; // case 203
altb2 11:d43f8b421d6d 309 case 220: // switch laser on/off
altb2 7:942fd77d5e19 310 if(N != 1)
altb2 7:942fd77d5e19 311 return false;
altb2 7:942fd77d5e19 312 switch(msg_id2)
altb2 7:942fd77d5e19 313 {
altb2 7:942fd77d5e19 314 case 1:
altb2 7:942fd77d5e19 315 if(buffer[7] == 1)
altb2 7:942fd77d5e19 316 data.laser_on = true;
altb2 7:942fd77d5e19 317 else
altb2 7:942fd77d5e19 318 data.laser_on = false;
altb2 7:942fd77d5e19 319 return true;
altb2 7:942fd77d5e19 320 break;
altb2 7:942fd77d5e19 321 }
altb2 7:942fd77d5e19 322 break;
altb2 11:d43f8b421d6d 323 case 221: // switch trafo on/off
altb2 7:942fd77d5e19 324 if(N != 1)
altb2 7:942fd77d5e19 325 return false;
altb2 7:942fd77d5e19 326 switch(msg_id2)
altb2 7:942fd77d5e19 327 {
altb2 7:942fd77d5e19 328 case 1:
altb2 7:942fd77d5e19 329 if(buffer[7] == 1)
altb2 7:942fd77d5e19 330 mk.trafo_is_on = true;
altb2 7:942fd77d5e19 331 else
altb2 7:942fd77d5e19 332 mk.trafo_is_on = false;
altb2 7:942fd77d5e19 333 return true;
altb2 7:942fd77d5e19 334 break;
altb2 7:942fd77d5e19 335 }
altb2 7:942fd77d5e19 336 break;
altb2 11:d43f8b421d6d 337 case 230: // set internal/external control
altb2 7:942fd77d5e19 338 if(N != 1)
altb2 7:942fd77d5e19 339 return false;
altb2 7:942fd77d5e19 340 switch(msg_id2)
altb2 7:942fd77d5e19 341 {
altb2 7:942fd77d5e19 342 case 1:
altb2 7:942fd77d5e19 343 if(buffer[7] == 1)
altb2 7:942fd77d5e19 344 mk.external_control = true;
altb2 7:942fd77d5e19 345 else
altb2 7:942fd77d5e19 346 mk.external_control = false;
altb2 7:942fd77d5e19 347 return true;
altb2 7:942fd77d5e19 348 break;
altb2 7:942fd77d5e19 349 }
altb2 7:942fd77d5e19 350 break;
altb2 7:942fd77d5e19 351 }
altb2 7:942fd77d5e19 352 return false;
altb2 7:942fd77d5e19 353 }
altb2 7:942fd77d5e19 354