Preston Ernst / Mbed OS mirror_actuator_VT

-data logging revision

Dependencies:   FastPWM

uart_comm_thread.cpp@2:92c25cb669f4, 2021-08-24 (annotated)

Committer:
ernstpre
Date:
Tue Aug 24 08:51:13 2021 +0000
Revision:
2:92c25cb669f4
Parent:
0:d2e117716219 
Publish Commit 24/8/21

Who changed what in which revision?

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