Ruprecht Altenburger / Mbed OS mirror_actuator_V1

Mirror actuator for RT2 lab

Dependencies:   FastPWM

uart_comm_thread.cpp@16:28b6bb8a4b7f, 2021-05-02 (annotated)

Committer:
altb2
Date:
Sun May 02 08:55:44 2021 +0000
Revision:
16:28b6bb8a4b7f
Parent:
15:9f32f64eee5b 
Final commit 4 students

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 15:9f32f64eee5b 97 uint16_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 15:9f32f64eee5b 115 send_state = 1012;
altb2 15:9f32f64eee5b 116 break;
altb2 15:9f32f64eee5b 117 case 1012:
altb2 11:d43f8b421d6d 118 send_f_data(101,34,2*4,data.est_xy); // send actual xy values
altb2 11:d43f8b421d6d 119 send_state = 202;
altb2 7:942fd77d5e19 120 break;
altb2 11:d43f8b421d6d 121 case 121: // currently omitted
altb2 11:d43f8b421d6d 122 send_data(121,34,2*2,mk.inc_additional_offset);
altb2 7:942fd77d5e19 123 send_state = 202;
altb2 7:942fd77d5e19 124 break;
altb2 11:d43f8b421d6d 125 case 202: // desired phi
altb2 11:d43f8b421d6d 126 send_f_data(202,12,2*4,data.cntrl_phi_des);
altb2 15:9f32f64eee5b 127 send_state = 125;
altb2 8:49ac75c42da0 128 break;
altb2 11:d43f8b421d6d 129 case 125: // number of iterations in the trafo
altb2 15:9f32f64eee5b 130 send_char_data(125,1,data.num_it+50);
altb2 11:d43f8b421d6d 131 send_state = 250;
altb2 11:d43f8b421d6d 132 break;
altb2 11:d43f8b421d6d 133 case 250: // send GPA values
altb2 8:49ac75c42da0 134 if(myGPA.new_data_available)
altb2 8:49ac75c42da0 135 {
altb2 8:49ac75c42da0 136 float dum[8];
altb2 8:49ac75c42da0 137 myGPA.getGPAdata(dum);
altb2 11:d43f8b421d6d 138 send_f_data(250,1,(uint16_t)32,dum); // send new values (8 floats)
altb2 8:49ac75c42da0 139 }
altb2 8:49ac75c42da0 140 else if(myGPA.start_now)
altb2 8:49ac75c42da0 141 {
altb2 11:d43f8b421d6d 142 send_char_data(250,2,0); // send start flag
altb2 8:49ac75c42da0 143 myGPA.start_now = false;
altb2 8:49ac75c42da0 144 gpa_stop_sent = false;
altb2 8:49ac75c42da0 145 }
altb2 8:49ac75c42da0 146 else if(myGPA.meas_is_finished && !gpa_stop_sent && !myGPA.new_data_available)
altb2 8:49ac75c42da0 147 {
altb2 11:d43f8b421d6d 148 send_char_data(250,255,1); // send stop flag
altb2 8:49ac75c42da0 149 gpa_stop_sent = true;
altb2 8:49ac75c42da0 150 }
altb2 7:942fd77d5e19 151 send_state = 101;
altb2 8:49ac75c42da0 152 break;
altb2 7:942fd77d5e19 153 default:
altb2 7:942fd77d5e19 154 break;
altb2 7:942fd77d5e19 155 }
altb2 7:942fd77d5e19 156 }// loop
altb2 7:942fd77d5e19 157 }
altb2 7:942fd77d5e19 158
altb2 7:942fd77d5e19 159
altb2 7:942fd77d5e19 160
altb2 7:942fd77d5e19 161 // #### receive data from hardware buffer, ensuring no dropped bytes
altb2 7:942fd77d5e19 162
altb2 7:942fd77d5e19 163 // ------------------- start uart ----------------
altb2 7:942fd77d5e19 164 void uart_comm_thread::start_uart(void){
altb2 7:942fd77d5e19 165
altb2 7:942fd77d5e19 166 thread.start(callback(this, &uart_comm_thread::run));
altb2 7:942fd77d5e19 167 ticker.attach(callback(this, &uart_comm_thread::sendThreadFlag), Ts);
altb2 7:942fd77d5e19 168 }
altb2 7:942fd77d5e19 169 // this is for realtime OS
altb2 7:942fd77d5e19 170 void uart_comm_thread::sendThreadFlag() {
altb2 7:942fd77d5e19 171 thread.flags_set(threadFlag);
altb2 7:942fd77d5e19 172 }
altb2 7:942fd77d5e19 173
altb2 8:49ac75c42da0 174 void uart_comm_thread::send_f_data(char id1,char id2,uint16_t N,float *dat)
altb2 7:942fd77d5e19 175 {
altb2 7:942fd77d5e19 176 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 7:942fd77d5e19 177 buffer[3] = id1;
altb2 7:942fd77d5e19 178 buffer[4] = id2;
altb2 8:49ac75c42da0 179 buffer[5] = N%256;
altb2 8:49ac75c42da0 180 buffer[6] = N/256;
altb2 7:942fd77d5e19 181 uart->write(buffer, 7);
altb2 7:942fd77d5e19 182 char *float_data = (char *)dat;
altb2 7:942fd77d5e19 183 uart->write(float_data,N);
altb2 7:942fd77d5e19 184 uart->write("\r",2); // line end
altb2 7:942fd77d5e19 185 }
altb2 11:d43f8b421d6d 186 // send uint16 data
altb2 7:942fd77d5e19 187 void uart_comm_thread::send_data(char id1,char id2,uint16_t N,int16_t *dat)
altb2 7:942fd77d5e19 188 {
altb2 7:942fd77d5e19 189 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 7:942fd77d5e19 190 buffer[3] = id1;
altb2 7:942fd77d5e19 191 buffer[4] = id2;
altb2 7:942fd77d5e19 192 buffer[5] = *(char *)N;
altb2 7:942fd77d5e19 193 uart->write(buffer, 7);
altb2 7:942fd77d5e19 194 char *int_data = (char *)dat;
altb2 7:942fd77d5e19 195 uart->write(int_data,N);
altb2 7:942fd77d5e19 196 uart->write("\r",2); // line end
altb2 7:942fd77d5e19 197 }
altb2 11:d43f8b421d6d 198 // send one uint16 data
altb2 7:942fd77d5e19 199 void uart_comm_thread::send_data(char id1,char id2,int16_t dat)
altb2 7:942fd77d5e19 200 {
altb2 7:942fd77d5e19 201 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 7:942fd77d5e19 202 buffer[3] = id1;
altb2 7:942fd77d5e19 203 buffer[4] = id2;
altb2 7:942fd77d5e19 204 buffer[5] = 2;
altb2 7:942fd77d5e19 205 buffer[6] = 0;
altb2 7:942fd77d5e19 206 uart->write(buffer, 7);
altb2 7:942fd77d5e19 207 char *int_data = (char *)dat;
altb2 7:942fd77d5e19 208 uart->write(int_data,2);
altb2 7:942fd77d5e19 209 uart->write("\r",2); // line end
altb2 7:942fd77d5e19 210 }
altb2 11:d43f8b421d6d 211 // send text
altb2 8:49ac75c42da0 212 void uart_comm_thread::send_char_data(char id1,char id2,uint8_t dat)
altb2 7:942fd77d5e19 213 {
altb2 7:942fd77d5e19 214 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 7:942fd77d5e19 215 buffer[3] = id1;
altb2 7:942fd77d5e19 216 buffer[4] = id2;
altb2 7:942fd77d5e19 217 buffer[5] = 1;
altb2 7:942fd77d5e19 218 buffer[6] = 0;
altb2 7:942fd77d5e19 219 uart->write(buffer, 7);
altb2 7:942fd77d5e19 220 char int8_data = (char)dat;
altb2 7:942fd77d5e19 221 uart->write(&int8_data,1);
altb2 7:942fd77d5e19 222 uart->write("\r",2); // line end
altb2 7:942fd77d5e19 223 }
altb2 8:49ac75c42da0 224 void uart_comm_thread::send_text(const char *txt)
altb2 8:49ac75c42da0 225 {
altb2 8:49ac75c42da0 226 uint16_t N=0;
altb2 11:d43f8b421d6d 227 while(txt[N] != '\0') // get length of text
altb2 8:49ac75c42da0 228 N++;
altb2 8:49ac75c42da0 229 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 8:49ac75c42da0 230 buffer[3] = 241;
altb2 8:49ac75c42da0 231 buffer[4] = 1;
altb2 8:49ac75c42da0 232 buffer[5] = *(char *)N;
altb2 8:49ac75c42da0 233 uart->write(buffer, 7);
altb2 8:49ac75c42da0 234 uart->write(txt,N);
altb2 8:49ac75c42da0 235 uart->write("\r",2); // line end
altb2 8:49ac75c42da0 236 }
altb2 11:d43f8b421d6d 237 // -----------------------------------------------------------------------------
altb2 11:d43f8b421d6d 238 // analyse data, see comments at top of this file for numbering
altb2 7:942fd77d5e19 239 bool uart_comm_thread::analyse_received_data(void){
altb2 7:942fd77d5e19 240 char msg_id1 = buffer[3];
altb2 7:942fd77d5e19 241 char msg_id2 = buffer[4];
altb2 7:942fd77d5e19 242 uint16_t N = 256 * buffer[6] + buffer[5];
altb2 7:942fd77d5e19 243 switch(msg_id1)
altb2 7:942fd77d5e19 244 {
altb2 11:d43f8b421d6d 245 case 201: // add additional offsets to offsets
altb2 7:942fd77d5e19 246 if(N != 2)
altb2 7:942fd77d5e19 247 return false;
altb2 7:942fd77d5e19 248 switch(msg_id2)
altb2 7:942fd77d5e19 249 {
altb2 11:d43f8b421d6d 250 case 1: // for phi1
altb2 7:942fd77d5e19 251 mk.add_additional_offsets(256 * buffer[8] + buffer[7],0);
altb2 7:942fd77d5e19 252 return true;
altb2 7:942fd77d5e19 253 break;
altb2 11:d43f8b421d6d 254 case 2: // for phi2
altb2 7:942fd77d5e19 255 mk.add_additional_offsets(0,256 * buffer[8] + buffer[7]);
altb2 7:942fd77d5e19 256 return true;
altb2 7:942fd77d5e19 257 break;
altb2 7:942fd77d5e19 258 default:
altb2 7:942fd77d5e19 259 break;
altb2 7:942fd77d5e19 260 }
altb2 7:942fd77d5e19 261 break; // case 201
altb2 11:d43f8b421d6d 262 case 202: // set desired phi or xy-values
altb2 7:942fd77d5e19 263 if(N != 4)
altb2 7:942fd77d5e19 264 return false;
altb2 7:942fd77d5e19 265 switch(msg_id2)
altb2 7:942fd77d5e19 266 {
altb2 7:942fd77d5e19 267 case 1:
altb2 7:942fd77d5e19 268 data.cntrl_phi_des[0] = *(float *)&buffer[7];
altb2 7:942fd77d5e19 269 return true;
altb2 7:942fd77d5e19 270 break;
altb2 7:942fd77d5e19 271 case 2:
altb2 7:942fd77d5e19 272 data.cntrl_phi_des[1] = *(float *)&buffer[7];
altb2 7:942fd77d5e19 273 return true;
altb2 7:942fd77d5e19 274 break;
altb2 7:942fd77d5e19 275 case 3:
altb2 11:d43f8b421d6d 276 data.cntrl_xy_des[0] = *(float *)&buffer[7];
altb2 7:942fd77d5e19 277 return true;
altb2 7:942fd77d5e19 278 break;
altb2 7:942fd77d5e19 279 case 4:
altb2 7:942fd77d5e19 280 data.cntrl_xy_des[1] = *(float *)&buffer[7];
altb2 7:942fd77d5e19 281 return true;
altb2 7:942fd77d5e19 282 break;
altb2 7:942fd77d5e19 283 default:
altb2 7:942fd77d5e19 284 break;
altb2 7:942fd77d5e19 285 }
altb2 7:942fd77d5e19 286 break; // case 202
altb2 11:d43f8b421d6d 287 case 203: // increment desired values
altb2 7:942fd77d5e19 288 if(N != 4)
altb2 7:942fd77d5e19 289 return false;
altb2 7:942fd77d5e19 290 switch(msg_id2)
altb2 7:942fd77d5e19 291 {
altb2 7:942fd77d5e19 292 case 1:
altb2 7:942fd77d5e19 293 data.cntrl_phi_des[0] += *(float *)&buffer[7];
altb2 7:942fd77d5e19 294 return true;
altb2 7:942fd77d5e19 295 break;
altb2 7:942fd77d5e19 296 case 2:
altb2 7:942fd77d5e19 297 data.cntrl_phi_des[1] += *(float *)&buffer[7];
altb2 7:942fd77d5e19 298 return true;
altb2 7:942fd77d5e19 299 break;
altb2 7:942fd77d5e19 300 case 3:
altb2 11:d43f8b421d6d 301 data.cntrl_xy_des[0] += *(float *)&buffer[7];
altb2 7:942fd77d5e19 302 return true;
altb2 7:942fd77d5e19 303 break;
altb2 7:942fd77d5e19 304 case 4:
altb2 7:942fd77d5e19 305 data.cntrl_xy_des[1] += *(float *)&buffer[7];
altb2 7:942fd77d5e19 306 return true;
altb2 7:942fd77d5e19 307 break;
altb2 7:942fd77d5e19 308 default:
altb2 7:942fd77d5e19 309 break;
altb2 7:942fd77d5e19 310 }
altb2 7:942fd77d5e19 311 break; // case 203
altb2 11:d43f8b421d6d 312 case 220: // switch laser on/off
altb2 7:942fd77d5e19 313 if(N != 1)
altb2 7:942fd77d5e19 314 return false;
altb2 7:942fd77d5e19 315 switch(msg_id2)
altb2 7:942fd77d5e19 316 {
altb2 7:942fd77d5e19 317 case 1:
altb2 7:942fd77d5e19 318 if(buffer[7] == 1)
altb2 7:942fd77d5e19 319 data.laser_on = true;
altb2 7:942fd77d5e19 320 else
altb2 7:942fd77d5e19 321 data.laser_on = false;
altb2 7:942fd77d5e19 322 return true;
altb2 7:942fd77d5e19 323 break;
altb2 7:942fd77d5e19 324 }
altb2 7:942fd77d5e19 325 break;
altb2 11:d43f8b421d6d 326 case 221: // switch trafo on/off
altb2 7:942fd77d5e19 327 if(N != 1)
altb2 7:942fd77d5e19 328 return false;
altb2 7:942fd77d5e19 329 switch(msg_id2)
altb2 7:942fd77d5e19 330 {
altb2 7:942fd77d5e19 331 case 1:
altb2 7:942fd77d5e19 332 if(buffer[7] == 1)
altb2 15:9f32f64eee5b 333 {
altb2 7:942fd77d5e19 334 mk.trafo_is_on = true;
altb2 15:9f32f64eee5b 335 send_text((char *)"switched Trafo on");
altb2 15:9f32f64eee5b 336 }
altb2 15:9f32f64eee5b 337 else
altb2 15:9f32f64eee5b 338 {
altb2 7:942fd77d5e19 339 mk.trafo_is_on = false;
altb2 15:9f32f64eee5b 340 send_text((char *)"switched Trafo off");
altb2 15:9f32f64eee5b 341 }
altb2 7:942fd77d5e19 342 return true;
altb2 7:942fd77d5e19 343 break;
altb2 7:942fd77d5e19 344 }
altb2 7:942fd77d5e19 345 break;
altb2 11:d43f8b421d6d 346 case 230: // set internal/external control
altb2 7:942fd77d5e19 347 if(N != 1)
altb2 7:942fd77d5e19 348 return false;
altb2 7:942fd77d5e19 349 switch(msg_id2)
altb2 7:942fd77d5e19 350 {
altb2 7:942fd77d5e19 351 case 1:
altb2 7:942fd77d5e19 352 if(buffer[7] == 1)
altb2 7:942fd77d5e19 353 mk.external_control = true;
altb2 7:942fd77d5e19 354 else
altb2 7:942fd77d5e19 355 mk.external_control = false;
altb2 7:942fd77d5e19 356 return true;
altb2 7:942fd77d5e19 357 break;
altb2 7:942fd77d5e19 358 }
altb2 7:942fd77d5e19 359 break;
altb2 7:942fd77d5e19 360 }
altb2 7:942fd77d5e19 361 return false;
altb2 7:942fd77d5e19 362 }
altb2 7:942fd77d5e19 363