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