Ruprecht Altenburger / Mbed OS mirror_actuator_V1

Mirror actuator for RT2 lab

Dependencies:   FastPWM

uart_comm_thread.cpp@8:49ac75c42da0, 2021-04-27 (annotated)

Committer:
altb2
Date:
Tue Apr 27 07:50:03 2021 +0000
Revision:
8:49ac75c42da0
Parent:
7:942fd77d5e19
Child:
11:d43f8b421d6d
intermediate;

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 }
altb2 7:942fd77d5e19 86
altb2 7:942fd77d5e19 87 // #### request data from device
altb2 7:942fd77d5e19 88 void uart_comm_thread::init(){
altb2 7:942fd77d5e19 89 // init statemachine
altb2 7:942fd77d5e19 90 }
altb2 7:942fd77d5e19 91
altb2 7:942fd77d5e19 92
altb2 7:942fd77d5e19 93 // #### run the statemachine
altb2 7:942fd77d5e19 94 void uart_comm_thread::run(void)
altb2 7:942fd77d5e19 95 {
altb2 7:942fd77d5e19 96 // returnvalue
altb2 7:942fd77d5e19 97 bool retVal = false;
altb2 7:942fd77d5e19 98 uint8_t checksum,k;
altb2 7:942fd77d5e19 99 uint8_t send_state =101;
altb2 7:942fd77d5e19 100 while(true)
altb2 7:942fd77d5e19 101 {
altb2 7:942fd77d5e19 102 ThisThread::flags_wait_any(threadFlag);
altb2 7:942fd77d5e19 103 //--- The LOOP --------------------------------------------------------
altb2 7:942fd77d5e19 104 uint32_t num = uart->read(buffer, sizeof(buffer));
altb2 7:942fd77d5e19 105 if (num >0)
altb2 7:942fd77d5e19 106 {
altb2 7:942fd77d5e19 107 if(buffer[0] == 254 && buffer[1] == 1)
altb2 7:942fd77d5e19 108 {
altb2 7:942fd77d5e19 109 if(analyse_received_data())
altb2 7:942fd77d5e19 110 ;// led1 = !led1;
altb2 7:942fd77d5e19 111 }
altb2 7:942fd77d5e19 112 }
altb2 7:942fd77d5e19 113 switch(send_state)
altb2 7:942fd77d5e19 114 {
altb2 7:942fd77d5e19 115 case 101:
altb2 8:49ac75c42da0 116 send_f_data(101,12,2*4,data.sens_phi); // done in every 2nd cycle!
altb2 8:49ac75c42da0 117 send_f_data(101,34,2*4,data.est_xy); // done in every 2nd cycle!
altb2 7:942fd77d5e19 118 send_state = 121;
altb2 7:942fd77d5e19 119 break;
altb2 7:942fd77d5e19 120 case 121:
altb2 7:942fd77d5e19 121 send_data(121,34,2*2,mk.inc_additional_offset); // done in every 2nd cycle!
altb2 7:942fd77d5e19 122 send_state = 202;
altb2 7:942fd77d5e19 123 break;
altb2 7:942fd77d5e19 124 case 202:
altb2 8:49ac75c42da0 125 send_f_data(202,12,2*4,data.cntrl_phi_des); // done in every 2nd cycle!
altb2 7:942fd77d5e19 126 send_state = 125;
altb2 7:942fd77d5e19 127 break;
altb2 7:942fd77d5e19 128 case 125:
altb2 7:942fd77d5e19 129 send_data(125,1,data.num_it); // done in every 2nd cycle!
altb2 8:49ac75c42da0 130 send_state = 250;
altb2 8:49ac75c42da0 131 break;
altb2 8:49ac75c42da0 132 case 250:
altb2 8:49ac75c42da0 133 if(myGPA.new_data_available)
altb2 8:49ac75c42da0 134 {
altb2 8:49ac75c42da0 135 float dum[8];
altb2 8:49ac75c42da0 136 myGPA.getGPAdata(dum);
altb2 8:49ac75c42da0 137 send_f_data(250,1,(uint16_t)32,dum);
altb2 8:49ac75c42da0 138 }
altb2 8:49ac75c42da0 139 else if(myGPA.start_now)
altb2 8:49ac75c42da0 140 {
altb2 8:49ac75c42da0 141 send_char_data(250,2,0);
altb2 8:49ac75c42da0 142 myGPA.start_now = false;
altb2 8:49ac75c42da0 143 gpa_stop_sent = false;
altb2 8:49ac75c42da0 144 }
altb2 8:49ac75c42da0 145 else if(myGPA.meas_is_finished && !gpa_stop_sent && !myGPA.new_data_available)
altb2 8:49ac75c42da0 146 {
altb2 8:49ac75c42da0 147 send_char_data(250,255,1);
altb2 8:49ac75c42da0 148 gpa_stop_sent = true;
altb2 8:49ac75c42da0 149 }
altb2 7:942fd77d5e19 150 send_state = 101;
altb2 8:49ac75c42da0 151 break;
altb2 7:942fd77d5e19 152 default:
altb2 7:942fd77d5e19 153 break;
altb2 7:942fd77d5e19 154 }
altb2 7:942fd77d5e19 155 }// loop
altb2 7:942fd77d5e19 156 }
altb2 7:942fd77d5e19 157
altb2 7:942fd77d5e19 158
altb2 7:942fd77d5e19 159
altb2 7:942fd77d5e19 160 // #### receive data from hardware buffer, ensuring no dropped bytes
altb2 7:942fd77d5e19 161
altb2 7:942fd77d5e19 162 // ------------------- start uart ----------------
altb2 7:942fd77d5e19 163 void uart_comm_thread::start_uart(void){
altb2 7:942fd77d5e19 164
altb2 7:942fd77d5e19 165 thread.start(callback(this, &uart_comm_thread::run));
altb2 7:942fd77d5e19 166 ticker.attach(callback(this, &uart_comm_thread::sendThreadFlag), Ts);
altb2 7:942fd77d5e19 167
altb2 7:942fd77d5e19 168 // thread.start(callback(this, &uart_comm_thread::run));
altb2 7:942fd77d5e19 169 // ticker.attach(callback(this, &uart_comm_thread::sendSignal), Ts);
altb2 7:942fd77d5e19 170 printf("UART Thread started\r\n");
altb2 7:942fd77d5e19 171
altb2 7:942fd77d5e19 172 }
altb2 7:942fd77d5e19 173 // this is for realtime OS
altb2 7:942fd77d5e19 174 void uart_comm_thread::sendThreadFlag() {
altb2 7:942fd77d5e19 175
altb2 7:942fd77d5e19 176 thread.flags_set(threadFlag);
altb2 7:942fd77d5e19 177 }
altb2 7:942fd77d5e19 178
altb2 8:49ac75c42da0 179 void uart_comm_thread::send_f_data(char id1,char id2,uint16_t N,float *dat)
altb2 7:942fd77d5e19 180 {
altb2 7:942fd77d5e19 181 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 7:942fd77d5e19 182 buffer[3] = id1;
altb2 7:942fd77d5e19 183 buffer[4] = id2;
altb2 8:49ac75c42da0 184 buffer[5] = N%256;
altb2 8:49ac75c42da0 185 buffer[6] = N/256;
altb2 7:942fd77d5e19 186 uart->write(buffer, 7);
altb2 7:942fd77d5e19 187 char *float_data = (char *)dat;
altb2 7:942fd77d5e19 188 uart->write(float_data,N);
altb2 7:942fd77d5e19 189 uart->write("\r",2); // line end
altb2 7:942fd77d5e19 190 //mutex.unlock();
altb2 7:942fd77d5e19 191 }
altb2 7:942fd77d5e19 192 void uart_comm_thread::send_data(char id1,char id2,uint16_t N,int16_t *dat)
altb2 7:942fd77d5e19 193 {
altb2 7:942fd77d5e19 194 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 7:942fd77d5e19 195 buffer[3] = id1;
altb2 7:942fd77d5e19 196 buffer[4] = id2;
altb2 7:942fd77d5e19 197 buffer[5] = *(char *)N;
altb2 7:942fd77d5e19 198 uart->write(buffer, 7);
altb2 7:942fd77d5e19 199 char *int_data = (char *)dat;
altb2 7:942fd77d5e19 200 uart->write(int_data,N);
altb2 7:942fd77d5e19 201 uart->write("\r",2); // line end
altb2 7:942fd77d5e19 202 //mutex.unlock();
altb2 7:942fd77d5e19 203 }
altb2 7:942fd77d5e19 204 void uart_comm_thread::send_data(char id1,char id2,int16_t dat)
altb2 7:942fd77d5e19 205 {
altb2 7:942fd77d5e19 206 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 7:942fd77d5e19 207 buffer[3] = id1;
altb2 7:942fd77d5e19 208 buffer[4] = id2;
altb2 7:942fd77d5e19 209 buffer[5] = 2;
altb2 7:942fd77d5e19 210 buffer[6] = 0;
altb2 7:942fd77d5e19 211 uart->write(buffer, 7);
altb2 7:942fd77d5e19 212 char *int_data = (char *)dat;
altb2 7:942fd77d5e19 213 uart->write(int_data,2);
altb2 7:942fd77d5e19 214 uart->write("\r",2); // line end
altb2 7:942fd77d5e19 215 //mutex.unlock();
altb2 7:942fd77d5e19 216 }
altb2 8:49ac75c42da0 217 void uart_comm_thread::send_char_data(char id1,char id2,uint8_t dat)
altb2 7:942fd77d5e19 218 {
altb2 7:942fd77d5e19 219 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 7:942fd77d5e19 220 buffer[3] = id1;
altb2 7:942fd77d5e19 221 buffer[4] = id2;
altb2 7:942fd77d5e19 222 buffer[5] = 1;
altb2 7:942fd77d5e19 223 buffer[6] = 0;
altb2 7:942fd77d5e19 224 uart->write(buffer, 7);
altb2 7:942fd77d5e19 225 char int8_data = (char)dat;
altb2 7:942fd77d5e19 226 uart->write(&int8_data,1);
altb2 7:942fd77d5e19 227 uart->write("\r",2); // line end
altb2 7:942fd77d5e19 228 //mutex.unlock();
altb2 7:942fd77d5e19 229 }
altb2 8:49ac75c42da0 230 void uart_comm_thread::send_text(const char *txt)
altb2 8:49ac75c42da0 231 {
altb2 8:49ac75c42da0 232 uint16_t N=0;
altb2 8:49ac75c42da0 233 while(txt[N] != '\0')
altb2 8:49ac75c42da0 234 N++;
altb2 8:49ac75c42da0 235 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 8:49ac75c42da0 236 buffer[3] = 241;
altb2 8:49ac75c42da0 237 buffer[4] = 1;
altb2 8:49ac75c42da0 238 buffer[5] = *(char *)N;
altb2 8:49ac75c42da0 239 uart->write(buffer, 7);
altb2 8:49ac75c42da0 240 // char int8_data = 66;
altb2 8:49ac75c42da0 241 // uart->write(&int8_data,1);
altb2 8:49ac75c42da0 242
altb2 8:49ac75c42da0 243 uart->write(txt,N);
altb2 8:49ac75c42da0 244 uart->write("\r",2); // line end
altb2 8:49ac75c42da0 245 //mutex.unlock();
altb2 8:49ac75c42da0 246 }
altb2 7:942fd77d5e19 247
altb2 7:942fd77d5e19 248 bool uart_comm_thread::analyse_received_data(void){
altb2 7:942fd77d5e19 249 char msg_id1 = buffer[3];
altb2 7:942fd77d5e19 250 char msg_id2 = buffer[4];
altb2 7:942fd77d5e19 251 uint16_t N = 256 * buffer[6] + buffer[5];
altb2 7:942fd77d5e19 252 switch(msg_id1)
altb2 7:942fd77d5e19 253 {
altb2 7:942fd77d5e19 254 case 201:
altb2 7:942fd77d5e19 255 if(N != 2)
altb2 7:942fd77d5e19 256 return false;
altb2 7:942fd77d5e19 257 switch(msg_id2)
altb2 7:942fd77d5e19 258 {
altb2 7:942fd77d5e19 259 case 1:
altb2 7:942fd77d5e19 260 mk.add_additional_offsets(256 * buffer[8] + buffer[7],0);
altb2 7:942fd77d5e19 261 return true;
altb2 7:942fd77d5e19 262 break;
altb2 7:942fd77d5e19 263 case 2:
altb2 7:942fd77d5e19 264 mk.add_additional_offsets(0,256 * buffer[8] + buffer[7]);
altb2 7:942fd77d5e19 265 return true;
altb2 7:942fd77d5e19 266 break;
altb2 7:942fd77d5e19 267 default:
altb2 7:942fd77d5e19 268 break;
altb2 7:942fd77d5e19 269 }
altb2 7:942fd77d5e19 270 break; // case 201
altb2 7:942fd77d5e19 271 case 202:
altb2 7:942fd77d5e19 272 if(N != 4)
altb2 7:942fd77d5e19 273 return false;
altb2 7:942fd77d5e19 274 switch(msg_id2)
altb2 7:942fd77d5e19 275 {
altb2 7:942fd77d5e19 276 case 1:
altb2 7:942fd77d5e19 277 data.cntrl_phi_des[0] = *(float *)&buffer[7];
altb2 7:942fd77d5e19 278 return true;
altb2 7:942fd77d5e19 279 break;
altb2 7:942fd77d5e19 280 case 2:
altb2 7:942fd77d5e19 281 data.cntrl_phi_des[1] = *(float *)&buffer[7];
altb2 7:942fd77d5e19 282 return true;
altb2 7:942fd77d5e19 283 break;
altb2 7:942fd77d5e19 284 case 3:
altb2 7:942fd77d5e19 285 data.cntrl_xy_des[1] = *(float *)&buffer[7];
altb2 7:942fd77d5e19 286 return true;
altb2 7:942fd77d5e19 287 break;
altb2 7:942fd77d5e19 288 case 4:
altb2 7:942fd77d5e19 289 data.cntrl_xy_des[1] = *(float *)&buffer[7];
altb2 7:942fd77d5e19 290 return true;
altb2 7:942fd77d5e19 291 break;
altb2 7:942fd77d5e19 292 default:
altb2 7:942fd77d5e19 293 break;
altb2 7:942fd77d5e19 294 }
altb2 7:942fd77d5e19 295 break; // case 202
altb2 7:942fd77d5e19 296 case 203:
altb2 7:942fd77d5e19 297 if(N != 4)
altb2 7:942fd77d5e19 298 return false;
altb2 7:942fd77d5e19 299 switch(msg_id2)
altb2 7:942fd77d5e19 300 {
altb2 7:942fd77d5e19 301 case 1:
altb2 7:942fd77d5e19 302 data.cntrl_phi_des[0] += *(float *)&buffer[7];
altb2 7:942fd77d5e19 303 return true;
altb2 7:942fd77d5e19 304 break;
altb2 7:942fd77d5e19 305 case 2:
altb2 7:942fd77d5e19 306 data.cntrl_phi_des[1] += *(float *)&buffer[7];
altb2 7:942fd77d5e19 307 return true;
altb2 7:942fd77d5e19 308 break;
altb2 7:942fd77d5e19 309 case 3:
altb2 7:942fd77d5e19 310 data.cntrl_xy_des[1] += *(float *)&buffer[7];
altb2 7:942fd77d5e19 311 return true;
altb2 7:942fd77d5e19 312 break;
altb2 7:942fd77d5e19 313 case 4:
altb2 7:942fd77d5e19 314 data.cntrl_xy_des[1] += *(float *)&buffer[7];
altb2 7:942fd77d5e19 315 return true;
altb2 7:942fd77d5e19 316 break;
altb2 7:942fd77d5e19 317 default:
altb2 7:942fd77d5e19 318 break;
altb2 7:942fd77d5e19 319 }
altb2 7:942fd77d5e19 320 break; // case 203
altb2 7:942fd77d5e19 321 case 220:
altb2 7:942fd77d5e19 322 if(N != 1)
altb2 7:942fd77d5e19 323 return false;
altb2 7:942fd77d5e19 324 switch(msg_id2)
altb2 7:942fd77d5e19 325 {
altb2 7:942fd77d5e19 326 case 1:
altb2 7:942fd77d5e19 327 if(buffer[7] == 1)
altb2 7:942fd77d5e19 328 data.laser_on = true;
altb2 7:942fd77d5e19 329 else
altb2 7:942fd77d5e19 330 data.laser_on = false;
altb2 7:942fd77d5e19 331 return true;
altb2 7:942fd77d5e19 332 break;
altb2 7:942fd77d5e19 333 }
altb2 7:942fd77d5e19 334 break;
altb2 7:942fd77d5e19 335 case 221:
altb2 7:942fd77d5e19 336 if(N != 1)
altb2 7:942fd77d5e19 337 return false;
altb2 7:942fd77d5e19 338 switch(msg_id2)
altb2 7:942fd77d5e19 339 {
altb2 7:942fd77d5e19 340 case 1:
altb2 7:942fd77d5e19 341 if(buffer[7] == 1)
altb2 7:942fd77d5e19 342 mk.trafo_is_on = true;
altb2 7:942fd77d5e19 343 else
altb2 7:942fd77d5e19 344 mk.trafo_is_on = false;
altb2 7:942fd77d5e19 345 return true;
altb2 7:942fd77d5e19 346 break;
altb2 7:942fd77d5e19 347 }
altb2 7:942fd77d5e19 348 break;
altb2 7:942fd77d5e19 349 case 230:
altb2 7:942fd77d5e19 350 if(N != 1)
altb2 7:942fd77d5e19 351 return false;
altb2 7:942fd77d5e19 352 switch(msg_id2)
altb2 7:942fd77d5e19 353 {
altb2 7:942fd77d5e19 354 case 1:
altb2 7:942fd77d5e19 355 if(buffer[7] == 1)
altb2 7:942fd77d5e19 356 mk.external_control = true;
altb2 7:942fd77d5e19 357 else
altb2 7:942fd77d5e19 358 mk.external_control = false;
altb2 7:942fd77d5e19 359 return true;
altb2 7:942fd77d5e19 360 break;
altb2 7:942fd77d5e19 361 }
altb2 7:942fd77d5e19 362 break;
altb2 7:942fd77d5e19 363 }
altb2 7:942fd77d5e19 364 return false;
altb2 7:942fd77d5e19 365 }
altb2 7:942fd77d5e19 366