Mirror actuator for RT2 lab
Dependencies: Library_Cntrl Library_Misc
Diff: uart_comm_thread.cpp
- Revision:
- 7:942fd77d5e19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/uart_comm_thread.cpp Thu Apr 15 05:36:55 2021 +0000 @@ -0,0 +1,320 @@ +// includes +#include "uart_comm_thread.h" +/* +-------- DATA PROTOCOL---------------------------- + 254 1 255 201 1 4 0 ... + n1 n2 rec id1 id2 #Byte1 #Byte2 thedata +-------------------------------------------------- +1-20 sensor values, + id1 id2 + 10 Counter values + 1 c1 + 2 c2 (increments) + 11 actual current + 1 i1 / A + 2 i2 / A +-------------------------------------------------- +// NOT USED: 21-40 cntrl values, 21 desired values 1 Phi1 / rad 2 Phi2 / rad 3 x / mm 4 y / mm +-------------------------------------------------- +101-120 estimates/actual values, + 101 angles and calculated x,y + 1 Phi1 / rad + 2 Phi2 / rad + 3 x / mm + 4 y / mm + +-------------------------------------------------- +121-140 send techn. values, like offsets + id1 id2 + 121 + 1 inc_offset phi1 / increments int16_t + 2 inc_offset phi2 / " " + 3 inc_additional_offset phi1 / increments int16_t + 4 inc_additional_offset phi2 / " " + 125 + 1 num_it of X2P trafo +-------------------------------------------------- + +2xx: set-value and commands (like disable controller...) + id1 id2 + 201: Set values/parameters + 1 inc_additional_offset phi1 / increments int16_t + 2 inc_additional_offset phi2 / " " + + 202: set desired absolute values + 1 phi1 rad float + 2 phi2 rad float + 3 x mm float + 4 y mm float + 203 Increment values + 1 dphi1 rad float + 2 dphi2 rad float + 3 dx mm float + 4 dy mm float + 220 Laser on/off + 1 0 = off, 1 = on + 221 Trafo on/off + 1 0 = off, 1 = on + 230 external control on/off + 1 0 = off, 1 = on + +*/ + +// #### constructor +uart_comm_thread::uart_comm_thread(BufferedSerial *com, float Ts): thread(osPriorityBelowNormal, 4096) + { + // init serial + uart = com; + //uart->attach(callback(this, &uart_comm_thread::callBack), RawSerial::RxIrq); + this->Ts = Ts; + init(); +} + +// #### destructor +uart_comm_thread::~uart_comm_thread() { + + +} + +// #### request data from device +void uart_comm_thread::init(){ + // init statemachine +} + + +// #### run the statemachine +void uart_comm_thread::run(void) +{ + // returnvalue + bool retVal = false; + uint8_t checksum,k; + uint8_t send_state =101; + while(true) + { + ThisThread::flags_wait_any(threadFlag); + //--- The LOOP -------------------------------------------------------- + uint32_t num = uart->read(buffer, sizeof(buffer)); + if (num >0) + { + if(buffer[0] == 254 && buffer[1] == 1) + { + if(analyse_received_data()) + ;// led1 = !led1; + } + } + switch(send_state) + { + case 101: + send_data(101,12,2*4,data.sens_phi); // done in every 2nd cycle! + send_data(101,34,2*4,data.est_xy); // done in every 2nd cycle! + send_state = 121; + break; + case 121: + send_data(121,34,2*2,mk.inc_additional_offset); // done in every 2nd cycle! + send_state = 202; + break; + case 202: + send_data(202,12,2*4,data.cntrl_phi_des); // done in every 2nd cycle! + send_state = 125; + break; + case 125: + send_data(125,1,data.num_it); // done in every 2nd cycle! + send_state = 101; + break; + default: + break; + } + }// loop +} + + + +// #### receive data from hardware buffer, ensuring no dropped bytes + +// ------------------- start uart ---------------- +void uart_comm_thread::start_uart(void){ + + thread.start(callback(this, &uart_comm_thread::run)); + ticker.attach(callback(this, &uart_comm_thread::sendThreadFlag), Ts); + + // thread.start(callback(this, &uart_comm_thread::run)); + // ticker.attach(callback(this, &uart_comm_thread::sendSignal), Ts); + printf("UART Thread started\r\n"); + +} +// this is for realtime OS +void uart_comm_thread::sendThreadFlag() { + + thread.flags_set(threadFlag); +} + +void uart_comm_thread::send_data(char id1,char id2,uint16_t N,float *dat) +{ + buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern + buffer[3] = id1; + buffer[4] = id2; + buffer[5] = *(char *)N; + uart->write(buffer, 7); + char *float_data = (char *)dat; + uart->write(float_data,N); + uart->write("\r",2); // line end + //mutex.unlock(); +} +void uart_comm_thread::send_data(char id1,char id2,uint16_t N,int16_t *dat) +{ + buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern + buffer[3] = id1; + buffer[4] = id2; + buffer[5] = *(char *)N; + uart->write(buffer, 7); + char *int_data = (char *)dat; + uart->write(int_data,N); + uart->write("\r",2); // line end + //mutex.unlock(); +} +void uart_comm_thread::send_data(char id1,char id2,int16_t dat) +{ + buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern + buffer[3] = id1; + buffer[4] = id2; + buffer[5] = 2; + buffer[6] = 0; + uart->write(buffer, 7); + char *int_data = (char *)dat; + uart->write(int_data,2); + uart->write("\r",2); // line end + //mutex.unlock(); +} +void uart_comm_thread::send_data(char id1,char id2,uint8_t dat) +{ + buffer[0]=254;buffer[1]=1;buffer[2]=255; // standard pattern + buffer[3] = id1; + buffer[4] = id2; + buffer[5] = 1; + buffer[6] = 0; + uart->write(buffer, 7); + char int8_data = (char)dat; + uart->write(&int8_data,1); + uart->write("\r",2); // line end + //mutex.unlock(); +} + +bool uart_comm_thread::analyse_received_data(void){ + char msg_id1 = buffer[3]; + char msg_id2 = buffer[4]; + uint16_t N = 256 * buffer[6] + buffer[5]; + switch(msg_id1) + { + case 201: + if(N != 2) + return false; + switch(msg_id2) + { + case 1: + mk.add_additional_offsets(256 * buffer[8] + buffer[7],0); + return true; + break; + case 2: + mk.add_additional_offsets(0,256 * buffer[8] + buffer[7]); + return true; + break; + default: + break; + } + break; // case 201 + case 202: + if(N != 4) + return false; + switch(msg_id2) + { + case 1: + data.cntrl_phi_des[0] = *(float *)&buffer[7]; + return true; + break; + case 2: + data.cntrl_phi_des[1] = *(float *)&buffer[7]; + return true; + break; + case 3: + data.cntrl_xy_des[1] = *(float *)&buffer[7]; + return true; + break; + case 4: + data.cntrl_xy_des[1] = *(float *)&buffer[7]; + return true; + break; + default: + break; + } + break; // case 202 + case 203: + if(N != 4) + return false; + switch(msg_id2) + { + case 1: + data.cntrl_phi_des[0] += *(float *)&buffer[7]; + return true; + break; + case 2: + data.cntrl_phi_des[1] += *(float *)&buffer[7]; + return true; + break; + case 3: + data.cntrl_xy_des[1] += *(float *)&buffer[7]; + return true; + break; + case 4: + data.cntrl_xy_des[1] += *(float *)&buffer[7]; + return true; + break; + default: + break; + } + break; // case 203 + case 220: + if(N != 1) + return false; + switch(msg_id2) + { + case 1: + if(buffer[7] == 1) + data.laser_on = true; + else + data.laser_on = false; + return true; + break; + } + break; + case 221: + if(N != 1) + return false; + switch(msg_id2) + { + case 1: + if(buffer[7] == 1) + mk.trafo_is_on = true; + else + mk.trafo_is_on = false; + return true; + break; + } + break; + case 230: + if(N != 1) + return false; + switch(msg_id2) + { + case 1: + if(buffer[7] == 1) + mk.external_control = true; + else + mk.external_control = false; + return true; + break; + } + break; + } + return false; +} +