Ruprecht Altenburger / Mbed OS mirror_actuator_V1

Mirror actuator for RT2 lab

Dependencies:   FastPWM

uart_comm_thread.cpp@7:942fd77d5e19, 2021-04-15 (annotated)

Committer:
altb2
Date:
Thu Apr 15 05:36:55 2021 +0000
Revision:
7:942fd77d5e19
Child:
8:49ac75c42da0
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 7:942fd77d5e19 60
altb2 7:942fd77d5e19 61 */
altb2 7:942fd77d5e19 62
altb2 7:942fd77d5e19 63 // #### constructor
altb2 7:942fd77d5e19 64 uart_comm_thread::uart_comm_thread(BufferedSerial *com, float Ts): thread(osPriorityBelowNormal, 4096)
altb2 7:942fd77d5e19 65 {
altb2 7:942fd77d5e19 66 // init serial
altb2 7:942fd77d5e19 67 uart = com;
altb2 7:942fd77d5e19 68 //uart->attach(callback(this, &uart_comm_thread::callBack), RawSerial::RxIrq);
altb2 7:942fd77d5e19 69 this->Ts = Ts;
altb2 7:942fd77d5e19 70 init();
altb2 7:942fd77d5e19 71 }
altb2 7:942fd77d5e19 72
altb2 7:942fd77d5e19 73 // #### destructor
altb2 7:942fd77d5e19 74 uart_comm_thread::~uart_comm_thread() {
altb2 7:942fd77d5e19 75
altb2 7:942fd77d5e19 76
altb2 7:942fd77d5e19 77 }
altb2 7:942fd77d5e19 78
altb2 7:942fd77d5e19 79 // #### request data from device
altb2 7:942fd77d5e19 80 void uart_comm_thread::init(){
altb2 7:942fd77d5e19 81 // init statemachine
altb2 7:942fd77d5e19 82 }
altb2 7:942fd77d5e19 83
altb2 7:942fd77d5e19 84
altb2 7:942fd77d5e19 85 // #### run the statemachine
altb2 7:942fd77d5e19 86 void uart_comm_thread::run(void)
altb2 7:942fd77d5e19 87 {
altb2 7:942fd77d5e19 88 // returnvalue
altb2 7:942fd77d5e19 89 bool retVal = false;
altb2 7:942fd77d5e19 90 uint8_t checksum,k;
altb2 7:942fd77d5e19 91 uint8_t send_state =101;
altb2 7:942fd77d5e19 92 while(true)
altb2 7:942fd77d5e19 93 {
altb2 7:942fd77d5e19 94 ThisThread::flags_wait_any(threadFlag);
altb2 7:942fd77d5e19 95 //--- The LOOP --------------------------------------------------------
altb2 7:942fd77d5e19 96 uint32_t num = uart->read(buffer, sizeof(buffer));
altb2 7:942fd77d5e19 97 if (num >0)
altb2 7:942fd77d5e19 98 {
altb2 7:942fd77d5e19 99 if(buffer[0] == 254 && buffer[1] == 1)
altb2 7:942fd77d5e19 100 {
altb2 7:942fd77d5e19 101 if(analyse_received_data())
altb2 7:942fd77d5e19 102 ;// led1 = !led1;
altb2 7:942fd77d5e19 103 }
altb2 7:942fd77d5e19 104 }
altb2 7:942fd77d5e19 105 switch(send_state)
altb2 7:942fd77d5e19 106 {
altb2 7:942fd77d5e19 107 case 101:
altb2 7:942fd77d5e19 108 send_data(101,12,2*4,data.sens_phi); // done in every 2nd cycle!
altb2 7:942fd77d5e19 109 send_data(101,34,2*4,data.est_xy); // done in every 2nd cycle!
altb2 7:942fd77d5e19 110 send_state = 121;
altb2 7:942fd77d5e19 111 break;
altb2 7:942fd77d5e19 112 case 121:
altb2 7:942fd77d5e19 113 send_data(121,34,2*2,mk.inc_additional_offset); // done in every 2nd cycle!
altb2 7:942fd77d5e19 114 send_state = 202;
altb2 7:942fd77d5e19 115 break;
altb2 7:942fd77d5e19 116 case 202:
altb2 7:942fd77d5e19 117 send_data(202,12,2*4,data.cntrl_phi_des); // done in every 2nd cycle!
altb2 7:942fd77d5e19 118 send_state = 125;
altb2 7:942fd77d5e19 119 break;
altb2 7:942fd77d5e19 120 case 125:
altb2 7:942fd77d5e19 121 send_data(125,1,data.num_it); // done in every 2nd cycle!
altb2 7:942fd77d5e19 122 send_state = 101;
altb2 7:942fd77d5e19 123 break;
altb2 7:942fd77d5e19 124 default:
altb2 7:942fd77d5e19 125 break;
altb2 7:942fd77d5e19 126 }
altb2 7:942fd77d5e19 127 }// loop
altb2 7:942fd77d5e19 128 }
altb2 7:942fd77d5e19 129
altb2 7:942fd77d5e19 130
altb2 7:942fd77d5e19 131
altb2 7:942fd77d5e19 132 // #### receive data from hardware buffer, ensuring no dropped bytes
altb2 7:942fd77d5e19 133
altb2 7:942fd77d5e19 134 // ------------------- start uart ----------------
altb2 7:942fd77d5e19 135 void uart_comm_thread::start_uart(void){
altb2 7:942fd77d5e19 136
altb2 7:942fd77d5e19 137 thread.start(callback(this, &uart_comm_thread::run));
altb2 7:942fd77d5e19 138 ticker.attach(callback(this, &uart_comm_thread::sendThreadFlag), Ts);
altb2 7:942fd77d5e19 139
altb2 7:942fd77d5e19 140 // thread.start(callback(this, &uart_comm_thread::run));
altb2 7:942fd77d5e19 141 // ticker.attach(callback(this, &uart_comm_thread::sendSignal), Ts);
altb2 7:942fd77d5e19 142 printf("UART Thread started\r\n");
altb2 7:942fd77d5e19 143
altb2 7:942fd77d5e19 144 }
altb2 7:942fd77d5e19 145 // this is for realtime OS
altb2 7:942fd77d5e19 146 void uart_comm_thread::sendThreadFlag() {
altb2 7:942fd77d5e19 147
altb2 7:942fd77d5e19 148 thread.flags_set(threadFlag);
altb2 7:942fd77d5e19 149 }
altb2 7:942fd77d5e19 150
altb2 7:942fd77d5e19 151 void uart_comm_thread::send_data(char id1,char id2,uint16_t N,float *dat)
altb2 7:942fd77d5e19 152 {
altb2 7:942fd77d5e19 153 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 7:942fd77d5e19 154 buffer[3] = id1;
altb2 7:942fd77d5e19 155 buffer[4] = id2;
altb2 7:942fd77d5e19 156 buffer[5] = *(char *)N;
altb2 7:942fd77d5e19 157 uart->write(buffer, 7);
altb2 7:942fd77d5e19 158 char *float_data = (char *)dat;
altb2 7:942fd77d5e19 159 uart->write(float_data,N);
altb2 7:942fd77d5e19 160 uart->write("\r",2); // line end
altb2 7:942fd77d5e19 161 //mutex.unlock();
altb2 7:942fd77d5e19 162 }
altb2 7:942fd77d5e19 163 void uart_comm_thread::send_data(char id1,char id2,uint16_t N,int16_t *dat)
altb2 7:942fd77d5e19 164 {
altb2 7:942fd77d5e19 165 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 7:942fd77d5e19 166 buffer[3] = id1;
altb2 7:942fd77d5e19 167 buffer[4] = id2;
altb2 7:942fd77d5e19 168 buffer[5] = *(char *)N;
altb2 7:942fd77d5e19 169 uart->write(buffer, 7);
altb2 7:942fd77d5e19 170 char *int_data = (char *)dat;
altb2 7:942fd77d5e19 171 uart->write(int_data,N);
altb2 7:942fd77d5e19 172 uart->write("\r",2); // line end
altb2 7:942fd77d5e19 173 //mutex.unlock();
altb2 7:942fd77d5e19 174 }
altb2 7:942fd77d5e19 175 void uart_comm_thread::send_data(char id1,char id2,int16_t dat)
altb2 7:942fd77d5e19 176 {
altb2 7:942fd77d5e19 177 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 7:942fd77d5e19 178 buffer[3] = id1;
altb2 7:942fd77d5e19 179 buffer[4] = id2;
altb2 7:942fd77d5e19 180 buffer[5] = 2;
altb2 7:942fd77d5e19 181 buffer[6] = 0;
altb2 7:942fd77d5e19 182 uart->write(buffer, 7);
altb2 7:942fd77d5e19 183 char *int_data = (char *)dat;
altb2 7:942fd77d5e19 184 uart->write(int_data,2);
altb2 7:942fd77d5e19 185 uart->write("\r",2); // line end
altb2 7:942fd77d5e19 186 //mutex.unlock();
altb2 7:942fd77d5e19 187 }
altb2 7:942fd77d5e19 188 void uart_comm_thread::send_data(char id1,char id2,uint8_t dat)
altb2 7:942fd77d5e19 189 {
altb2 7:942fd77d5e19 190 buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern
altb2 7:942fd77d5e19 191 buffer[3] = id1;
altb2 7:942fd77d5e19 192 buffer[4] = id2;
altb2 7:942fd77d5e19 193 buffer[5] = 1;
altb2 7:942fd77d5e19 194 buffer[6] = 0;
altb2 7:942fd77d5e19 195 uart->write(buffer, 7);
altb2 7:942fd77d5e19 196 char int8_data = (char)dat;
altb2 7:942fd77d5e19 197 uart->write(&int8_data,1);
altb2 7:942fd77d5e19 198 uart->write("\r",2); // line end
altb2 7:942fd77d5e19 199 //mutex.unlock();
altb2 7:942fd77d5e19 200 }
altb2 7:942fd77d5e19 201
altb2 7:942fd77d5e19 202 bool uart_comm_thread::analyse_received_data(void){
altb2 7:942fd77d5e19 203 char msg_id1 = buffer[3];
altb2 7:942fd77d5e19 204 char msg_id2 = buffer[4];
altb2 7:942fd77d5e19 205 uint16_t N = 256 * buffer[6] + buffer[5];
altb2 7:942fd77d5e19 206 switch(msg_id1)
altb2 7:942fd77d5e19 207 {
altb2 7:942fd77d5e19 208 case 201:
altb2 7:942fd77d5e19 209 if(N != 2)
altb2 7:942fd77d5e19 210 return false;
altb2 7:942fd77d5e19 211 switch(msg_id2)
altb2 7:942fd77d5e19 212 {
altb2 7:942fd77d5e19 213 case 1:
altb2 7:942fd77d5e19 214 mk.add_additional_offsets(256 * buffer[8] + buffer[7],0);
altb2 7:942fd77d5e19 215 return true;
altb2 7:942fd77d5e19 216 break;
altb2 7:942fd77d5e19 217 case 2:
altb2 7:942fd77d5e19 218 mk.add_additional_offsets(0,256 * buffer[8] + buffer[7]);
altb2 7:942fd77d5e19 219 return true;
altb2 7:942fd77d5e19 220 break;
altb2 7:942fd77d5e19 221 default:
altb2 7:942fd77d5e19 222 break;
altb2 7:942fd77d5e19 223 }
altb2 7:942fd77d5e19 224 break; // case 201
altb2 7:942fd77d5e19 225 case 202:
altb2 7:942fd77d5e19 226 if(N != 4)
altb2 7:942fd77d5e19 227 return false;
altb2 7:942fd77d5e19 228 switch(msg_id2)
altb2 7:942fd77d5e19 229 {
altb2 7:942fd77d5e19 230 case 1:
altb2 7:942fd77d5e19 231 data.cntrl_phi_des[0] = *(float *)&buffer[7];
altb2 7:942fd77d5e19 232 return true;
altb2 7:942fd77d5e19 233 break;
altb2 7:942fd77d5e19 234 case 2:
altb2 7:942fd77d5e19 235 data.cntrl_phi_des[1] = *(float *)&buffer[7];
altb2 7:942fd77d5e19 236 return true;
altb2 7:942fd77d5e19 237 break;
altb2 7:942fd77d5e19 238 case 3:
altb2 7:942fd77d5e19 239 data.cntrl_xy_des[1] = *(float *)&buffer[7];
altb2 7:942fd77d5e19 240 return true;
altb2 7:942fd77d5e19 241 break;
altb2 7:942fd77d5e19 242 case 4:
altb2 7:942fd77d5e19 243 data.cntrl_xy_des[1] = *(float *)&buffer[7];
altb2 7:942fd77d5e19 244 return true;
altb2 7:942fd77d5e19 245 break;
altb2 7:942fd77d5e19 246 default:
altb2 7:942fd77d5e19 247 break;
altb2 7:942fd77d5e19 248 }
altb2 7:942fd77d5e19 249 break; // case 202
altb2 7:942fd77d5e19 250 case 203:
altb2 7:942fd77d5e19 251 if(N != 4)
altb2 7:942fd77d5e19 252 return false;
altb2 7:942fd77d5e19 253 switch(msg_id2)
altb2 7:942fd77d5e19 254 {
altb2 7:942fd77d5e19 255 case 1:
altb2 7:942fd77d5e19 256 data.cntrl_phi_des[0] += *(float *)&buffer[7];
altb2 7:942fd77d5e19 257 return true;
altb2 7:942fd77d5e19 258 break;
altb2 7:942fd77d5e19 259 case 2:
altb2 7:942fd77d5e19 260 data.cntrl_phi_des[1] += *(float *)&buffer[7];
altb2 7:942fd77d5e19 261 return true;
altb2 7:942fd77d5e19 262 break;
altb2 7:942fd77d5e19 263 case 3:
altb2 7:942fd77d5e19 264 data.cntrl_xy_des[1] += *(float *)&buffer[7];
altb2 7:942fd77d5e19 265 return true;
altb2 7:942fd77d5e19 266 break;
altb2 7:942fd77d5e19 267 case 4:
altb2 7:942fd77d5e19 268 data.cntrl_xy_des[1] += *(float *)&buffer[7];
altb2 7:942fd77d5e19 269 return true;
altb2 7:942fd77d5e19 270 break;
altb2 7:942fd77d5e19 271 default:
altb2 7:942fd77d5e19 272 break;
altb2 7:942fd77d5e19 273 }
altb2 7:942fd77d5e19 274 break; // case 203
altb2 7:942fd77d5e19 275 case 220:
altb2 7:942fd77d5e19 276 if(N != 1)
altb2 7:942fd77d5e19 277 return false;
altb2 7:942fd77d5e19 278 switch(msg_id2)
altb2 7:942fd77d5e19 279 {
altb2 7:942fd77d5e19 280 case 1:
altb2 7:942fd77d5e19 281 if(buffer[7] == 1)
altb2 7:942fd77d5e19 282 data.laser_on = true;
altb2 7:942fd77d5e19 283 else
altb2 7:942fd77d5e19 284 data.laser_on = false;
altb2 7:942fd77d5e19 285 return true;
altb2 7:942fd77d5e19 286 break;
altb2 7:942fd77d5e19 287 }
altb2 7:942fd77d5e19 288 break;
altb2 7:942fd77d5e19 289 case 221:
altb2 7:942fd77d5e19 290 if(N != 1)
altb2 7:942fd77d5e19 291 return false;
altb2 7:942fd77d5e19 292 switch(msg_id2)
altb2 7:942fd77d5e19 293 {
altb2 7:942fd77d5e19 294 case 1:
altb2 7:942fd77d5e19 295 if(buffer[7] == 1)
altb2 7:942fd77d5e19 296 mk.trafo_is_on = true;
altb2 7:942fd77d5e19 297 else
altb2 7:942fd77d5e19 298 mk.trafo_is_on = false;
altb2 7:942fd77d5e19 299 return true;
altb2 7:942fd77d5e19 300 break;
altb2 7:942fd77d5e19 301 }
altb2 7:942fd77d5e19 302 break;
altb2 7:942fd77d5e19 303 case 230:
altb2 7:942fd77d5e19 304 if(N != 1)
altb2 7:942fd77d5e19 305 return false;
altb2 7:942fd77d5e19 306 switch(msg_id2)
altb2 7:942fd77d5e19 307 {
altb2 7:942fd77d5e19 308 case 1:
altb2 7:942fd77d5e19 309 if(buffer[7] == 1)
altb2 7:942fd77d5e19 310 mk.external_control = true;
altb2 7:942fd77d5e19 311 else
altb2 7:942fd77d5e19 312 mk.external_control = false;
altb2 7:942fd77d5e19 313 return true;
altb2 7:942fd77d5e19 314 break;
altb2 7:942fd77d5e19 315 }
altb2 7:942fd77d5e19 316 break;
altb2 7:942fd77d5e19 317 }
altb2 7:942fd77d5e19 318 return false;
altb2 7:942fd77d5e19 319 }
altb2 7:942fd77d5e19 320