com_rs485.cpp

00001 //
00002 // RUTINAS DE COMUNICACIONES POR RS485
00003 //
00004 
00005 #include "declaraciones.h"
00006 
00007 extern DigitalIn SEL0;                           // Selector 1 = BABOR/ESTRIBOR, ON=BABOR OFF=ESTRIBOR
00008 extern DigitalOut led3;                          // LED3
00009 extern st_datos_servo datos_servo;      // ESTRUCTURA DE VARIABLES DE ESTADO DEL SERVO DE DIRECCION
00010 extern st_datos_servo datos_otro_servo; // ESTRUCTURA DE VARIABLES DE ESTADO DEL OTRO SERVO DE DIRECCION
00011 
00012 extern Serial serRS485;
00013 extern Serial pc;
00014 extern DigitalOut RS485_DIR;
00015 extern long t_ult_recepcion;   // guarda tiempo ultima recepción
00016 extern volatile unsigned long tiempo_ms;    // Contador de ms
00017 extern int servo;              // servo recibido en ultima comunicacion
00018 extern volatile float latitud;
00019 extern volatile float longitud;
00020 extern volatile long fecha_hora;
00021 extern volatile int n_satelites;
00022 extern volatile float HDOP;
00023 extern volatile int rumbo;
00024 extern volatile int velocidad;  // MNPH
00025 extern volatile int timon_babor;
00026 extern volatile int timon_estribor;
00027 extern volatile enEstadoDireccion estado_direccion;
00028 extern volatile byte EstadoAnteriorServo;       //Guarda estado anterior del servo para salir de piloto-automatico o rumbo
00029 extern volatile int retardoActualizacion;       // para no visualizar estado durante este tiempo en ms
00030 extern volatile bool act_display;               // flag para actualizar display
00031 extern volatile bool trama_estado_valida;
00032 extern volatile bool trama_posicion_valida;
00033 extern volatile bool f_actualiza_tim_trans;     // Pone flag para actualizar timones y transmision
00034 
00035 
00036 extern int cfg_hora;  // Suma dos horas a UTC
00037 
00038 extern Timer timer;
00039 
00040 extern volatile bool f_rs485_busy;  // comunicacion ocupada
00041 extern int babor_estribor;          // 0=babor 1=estribor                
00042 
00043 extern int estado_mando;            // estado mando
00044 extern byte pulsadores;             // Estado pulsadores del mando
00045 
00046 extern volatile bool recibidoEstadoServo;      //Si ha recibido estado del otro servo se pone a true para sincronizacion
00047 extern Timer timerRS485;                       // Timer para sincronizacion por rs485 de los mandos
00048 
00049 // DEFINICION FUNCIONES
00050 uint16_t checksum(uint8_t *buf, uint16_t nb);   // CALCULA CHECKSUM
00051 
00052 volatile char tx_buffer[buffer_size+1];
00053 volatile char rx_buffer[buffer_size+1];
00054 
00055 volatile byte RS485BufIn[buffer_size+1];        // Buffer de copia de recepcion por RS485
00056 volatile int tiempo_tx_rs485=0;
00057 
00058 
00059 // Circular buffer pointers
00060 // volatile makes read-modify-write atomic
00061 volatile int tx_in=0;
00062 volatile int tx_out=0;
00063 
00064 void PrintHex8(uint8_t *data, uint8_t length); // prints 8-bit data in hex with leading zeroes
00065 
00066 long tim_envio = 0;     // tiempo en ms de envio de estado a mandos
00067 
00068 
00069 /*
00070 /////////////////////////////////////////////////////////
00071 // PROCESA RECEPCION DESDE SERIAL RS485
00072 void onSerialRxRS485(void){  // Procesa recepcion RS485
00073 
00074     printf("RS485:");
00075     while (serRS485.readable()) {
00076         char c = serRS485.getc();
00077         pc.putc(c);
00078     }
00079     printf("\r\n");
00080 }
00081 */
00082 
00083 
00084 
00085 /*
00086  * ENVIA ESTADO A LOS MANDOS
00087  */
00088 void envia_estado_mando(void){
00089     
00090     if(f_rs485_busy)       // SI YA ESTA TRANSMITIENDO, FINALIZA
00091         return;
00092 
00093     st_trama_estado *p_trama_estado;
00094     p_trama_estado = (st_trama_estado *)&tx_buffer;
00095     memset((void *)p_trama_estado, 0x00, sizeof(st_trama_estado));
00096     
00097     p_trama_estado->bStart = STX;
00098     p_trama_estado->nBytes = sizeof(st_trama_estado);
00099     p_trama_estado->servo = babor_estribor;  // Si es servo de babor = 0
00100     p_trama_estado->tipo = TIPO_TRAMA_ESTADO_RS485;
00101     p_trama_estado->HDOP = HDOP;
00102     p_trama_estado->nSatelites = n_satelites;
00103     p_trama_estado->latitud = datos_servo.latitud;
00104     p_trama_estado->longitud = datos_servo.longitud;
00105     p_trama_estado->timeStamp = datos_servo.fecha_hora + (long)cfg_hora;  // Suma ajuste hora;
00106     p_trama_estado->velocidad = datos_servo.velocidad;
00107     p_trama_estado->rumbo = datos_servo.rumbo;
00108     p_trama_estado->estado = datos_servo.estado;
00109     p_trama_estado->timBabor = datos_servo.timon_babor;
00110     p_trama_estado->timEstribor = datos_servo.timon_estribor;
00111     p_trama_estado->posicion = datos_servo.posicion_mando;
00112     p_trama_estado->trim_timon_bab = datos_servo.trim_timon_bab;
00113     p_trama_estado->trim_trans_bab = datos_servo.trim_trans_bab;
00114     p_trama_estado->trim_timon_estr = datos_servo.trim_timon_estr;
00115     p_trama_estado->trim_trans_estr = datos_servo.trim_trans_estr;
00116     p_trama_estado->rumbo_fijado = datos_servo.rumbo_fijado;
00117     p_trama_estado->ch_radio = datos_servo.ch_radio;    
00118     p_trama_estado->chk = checksum( (byte *)p_trama_estado, sizeof(st_trama_estado)-3);
00119     p_trama_estado->bStop = ETX;
00120     
00121 //    PrintHex8((uint8_t *)&tx_buffer, sizeof(st_trama_estado));
00122 //    pc.printf("\r\n");
00123 
00124     RS485_DIR = 1;  // pone en TX
00125     tx_out = 0;
00126     tx_in = sizeof(st_trama_estado);
00127     f_rs485_busy=true;     // TRANSMITIENDO
00128 
00129     led3 = 1;       // enciende led INICIO DE TRANSMISION
00130 
00131     printf("%dms envia estado: %d\r\n", millis()-tim_envio, datos_servo.estado);
00132     printf("trim_timon_bab:%d trim_timon_estr:%d\r\n", datos_servo.trim_timon_bab, datos_servo.trim_timon_estr);
00133 //    printf("%dms envia estado: %d\r\n", millis()-tim_envio, datos_servo.estado);
00134 //    printf("est:%d pos:%d, est:%d pos:%d\r\n", datos_servo.estado, (int16_t)datos_servo.posicion_mando, datos_otro_servo.estado, (int16_t)posicion_otro_servo);
00135 
00136     tim_envio = millis();
00137 }
00138 
00139 
00140 
00141 /*
00142  * ENVIA POSICION DEL MANDO
00143  */
00144 void envia_posicion_mando(uint16_t posicion_mando){
00145 
00146 /*
00147     led3 = 1;
00148 
00149     st_trama_posicion trama_posicion;
00150     memset((void *)&trama_posicion, 0x00, sizeof(trama_posicion));
00151     
00152     trama_posicion.bStart = STX;
00153     trama_posicion.nBytes = sizeof(st_trama_posicion);
00154     trama_posicion.servo = SEL0.read();  // Si es servo de babor = 0
00155     trama_posicion.tipo = TIPO_TRAMA_POSICION_RS485;
00156     trama_posicion.posicion = posicion_mando;
00157     trama_posicion.estado = (int)datos_servo.estado;
00158     trama_posicion.chk = checksum( (byte *)&trama_posicion, sizeof(trama_posicion)-3);
00159     trama_posicion.bStop = ETX;
00160     
00161     //  printTrama( (byte *)&trama_posicion, sizeof(trama_posicion));
00162     
00163     uint16_t i = sizeof(trama_posicion);
00164     byte *p = (byte *)&trama_posicion;
00165      
00166     RS485_DIR = 1;  // pone en TX
00167     wait(0.1);
00168     while(i-->0)
00169         serRS485.putc(*p++);
00170 //    serRS485.printf("PRUEBA DE COMUNICACION POR RS485\r\n");
00171     wait(0.1);
00172     RS485_DIR = 0;  // pone en RX
00173        
00174     printf("pos>> %d\r\n", trama_posicion.posicion);
00175     led3 = 0;
00176 */
00177 }
00178 
00179 
00180 
00181 
00182 /*
00183  * PROCESA TRAMA RECIBIDA DESDE EL MANDO
00184  */
00185 void procesaTramaMando(st_trama_estado_mando *p)
00186 {
00187 static uint8_t pulsadores_old = 0;
00188 static uint8_t rudBab_old = 0;
00189 static uint8_t rudEst_old = 0;
00190 static uint8_t traBab_old = 0;
00191 static uint8_t traEst_old = 0;
00192 
00193 
00194     if(p->mando == babor_estribor) {  // Si es de su mando lo procesa
00195         datos_servo.ch_radio = p->radioCH;
00196         datos_servo.trim_timon_bab = p->rudBab;
00197         datos_servo.trim_timon_estr = p->rudEst;
00198         datos_servo.trim_trans_bab = p->traBab;
00199         datos_servo.trim_trans_estr = p->traEst;
00200         estado_mando = p->estado;
00201         pulsadores = p->pulsado;    // Estado pulsadores del mando
00202 
00203         if(pulsadores != pulsadores_old){
00204             procesa_pulsadores(pulsadores);     // Procesa segun estado de los pulsadores del mando
00205             pulsadores_old = pulsadores;
00206         }
00207         
00208         if( rudBab_old != p->rudBab ||
00209             rudEst_old != p->rudEst ||
00210             traBab_old != p->traBab ||
00211             traEst_old != p->traEst ){
00212             f_actualiza_tim_trans = true;   // Pone flag para actualizar timones y transmision
00213         }
00214         
00215         rudBab_old = p->rudBab;
00216         rudEst_old = p->rudEst;
00217         traBab_old = p->traBab;
00218         traEst_old = p->traEst;
00219         
00220 
00221 //        printf("%dms estado_mando:%d\r\n", millis()-tim_envio, estado_mando);
00222 //        printf("trim_timon_bab:%d trim_timon_estr:%d\r\n", datos_servo.trim_timon_bab, datos_servo.trim_timon_estr);
00223 
00224 /*
00225         printf("pulsadores:%d\r\n", pulsadores);
00226         printf("%dms estado_mando:%d\r\n", millis()-tim_envio, estado_mando);
00227     } else {
00228         printf("%dms otro_mando\r\n", millis()-tim_envio);
00229 */
00230     }
00231 }
00232 
00233 
00234 
00235 
00236 
00237 ////////////////////////////////////////////////////////
00238 // ENVIA TEST A SALIDA RS485
00239 void envia_test(void)
00240 {
00241 
00242     RS485_DIR = 1;   // Envia
00243     wait(0.0005);    // espera 0.5 ms
00244 //        serRS485.printf("LED %s\r\n", (myled)?"Encendido" : "Apagado");
00245     serRS485.printf("SALIDA RS485\r\n");
00246     wait(0.0005);    // espera 0.5 ms
00247     RS485_DIR = 0;   // Recibe
00248 }
00249 
00250 
00251 
00252 
00253 /*
00254  * PROCESA TRAMA RECIBIDA DESDE EL SERVO
00255  */
00256 void procesaTramaServo(st_trama_estado *p)
00257 {
00258 
00259 //    char buf[150];
00260 
00261 //  sprintf(buf, "Aux:%d servo:%d\r\n", (nMando), p->servo);
00262 //  enviaRS485(buf);
00263     if(p->servo == babor_estribor) {  // Si es de su servo lo procesa, // 0=babor 1=estribor 
00264 /*
00265         servo = p->servo;
00266         HDOP = p->HDOP;
00267         n_satelites = p->nSatelites;
00268         latitud = p->latitud;
00269         longitud = p->longitud;
00270         fecha_hora = p->timeStamp;
00271         velocidad = int(p->velocidad);
00272         rumbo = int(p->rumbo);
00273         timon_babor = p->timBabor;
00274         timon_estribor = p->timEstribor;
00275         EstadoAnteriorServo = p->estado;
00276 */
00277 //        printf("%dms mismo_servo\r\n", millis()-tim_envio);
00278 
00279 
00280     } else { // PROCESA TRAMA DE ESTADO DEL OTRO SERVO, SI NO ESTÁ EN REPOSO Y EL SI, ACTUALIZA VALORES
00281 /*
00282         if(estado_direccion == DIR_REPOSO && p->estado != DIR_REPOSO) {
00283             latitud = p->latitud;
00284             longitud = p->longitud;
00285             fecha_hora = p->timeStamp;
00286             velocidad = int(p->velocidad);
00287             rumbo = int(p->rumbo);
00288             timon_babor = p->timBabor;
00289             timon_estribor = p->timEstribor;
00290             datos_otro_servo.estado = p->estado;
00291         }
00292 */
00293         recibidoEstadoServo=true;               //Si ha recibido estado del otro servo está a true para sincronizacion
00294         timerRS485.reset();
00295         // Guarda datos de estado del otro servo
00296         datos_otro_servo.n_servo = p->servo;
00297         datos_otro_servo.estado = p->estado;
00298         datos_otro_servo.latitud = p->latitud;
00299         datos_otro_servo.longitud = p->longitud;
00300         datos_otro_servo.fecha_hora = p->timeStamp;
00301         datos_otro_servo.velocidad = p->velocidad;          // NUDOS
00302         datos_otro_servo.rumbo = p->rumbo;
00303         datos_otro_servo.ch_radio = p->ch_radio;
00304         datos_otro_servo.trim_timon_bab = p->trim_timon_bab;
00305         datos_otro_servo.trim_trans_bab = p->trim_trans_bab;
00306         datos_otro_servo.trim_timon_estr = p->trim_timon_estr;
00307         datos_otro_servo.trim_trans_estr = p->trim_trans_estr;
00308         datos_otro_servo.timon_babor = p->timBabor;        // %de timon babor bajado, para 100% 5s, 1% = 50ms
00309         datos_otro_servo.timon_estribor = p->timEstribor;     // %de timon estribor bajado, para 100% 5s
00310         datos_otro_servo.rumbo_fijado = p->rumbo_fijado;       // Rumbo fijado al pulsar botón para RUMBO FIJO
00311         datos_otro_servo.rumbo_piloto_auto = p->rumbo_fijado;  // Rumbo fijado por piloto automático
00312         datos_otro_servo.posicion_mando = p->posicion;     // Posicion actual del mando
00313 
00314 /*        
00315     // SI LOS DOS EN REPOSO ACTUALIZAN CONFIGURACIONES
00316     if(datos_servo.estado == DIR_REPOSO && datos_otro_servo.estado == DIR_REPOSO){
00317         datos_servo.ch_radio = datos_otro_servo.ch_radio;
00318         datos_servo.trim_timon_bab = datos_otro_servo.trim_timon_bab;
00319         datos_servo.trim_trans_bab = datos_otro_servo.trim_trans_bab;
00320         datos_servo.trim_timon_estr = datos_otro_servo.trim_timon_estr;
00321         datos_servo.trim_trans_estr = datos_otro_servo.trim_trans_estr;
00322         datos_servo.rumbo_fijado = datos_otro_servo.rumbo_fijado;       // Rumbo fijado al pulsar botón para RUMBO FIJO
00323     }
00324 */
00325 
00326 
00327 //        PrintHex8((uint8_t*)&datos_otro_servo, sizeof datos_otro_servo); // prints 8-bit data in hex with leading zeroes
00328         printf("\r\n%d ms otro_servo:%02X\r\n\r\n", millis()-tim_envio, datos_otro_servo.estado);
00329         printf("trim_timon_bab:%d trim_timon_estr:%d\r\n", datos_otro_servo.trim_timon_bab, datos_otro_servo.trim_timon_estr);
00330     }
00331 }
00332 
00333 
00334 /*
00335  * PROCESA TRAMA DE POSICION RECIBIDA DESDE LOS SERVOS
00336  */
00337 void procesaTramaPosicion(st_trama_posicion *p)
00338 {
00339 
00340     if(p->servo == babor_estribor) {  // Si es del otro servo actualiza estado del otro
00341         printf("%dms pos_mismo_servo\r\n", millis()-tim_envio);
00342     }
00343     else{
00344         datos_otro_servo.estado = p->estado;
00345         printf("%dms pos_otro_servo\r\n", millis()-tim_envio);
00346     }
00347 }
00348 
00349 
00350 
00351 /////////////////////////////////////////////////////////
00352 // PROCESA RECEPCION DESDE RS485
00353 void onSerialRxRS485(void)   // Procesa recepcion RS485 por interrupcion
00354 {
00355     static uint16_t nc = 0;   // bytes recibidos
00356     static uint16_t nt = 0;   // bytes a recibir
00357     static bool inicio = false; // true si recibiendo trama
00358 
00359     while (serRS485.readable()) {
00360         char c = serRS485.getc();
00361 //        pc.putc(c);
00362         t_ult_recepcion = tiempo_ms;   // guarda tiempo ultima recepción
00363         if(inicio == true) {  // procesa caracter
00364             rx_buffer[nc] = c;
00365             if(nc==1)
00366                 nt = c;   // carga numero de bytes
00367             if(nc>0 && nc>=(nt-1)) { // final de trama
00368                 if(c == ETX) {    // si caracter final de trama
00369                     uint16_t chk = ((uint16_t)rx_buffer[nt-2]*256)+rx_buffer[nt-3];
00370                     uint16_t chkCal = checksum((uint8_t *)rx_buffer, nt-3);
00371                     
00372 //                    PrintHex8((uint8_t *)rx_buffer, nc+1); // prints 8-bit data in hex with leading zeroes
00373 //                    pc.printf("\r\nchk=%04X ", chk);
00374 //                    pc.printf("chkCal=%04X\r\n", chkCal);
00375                     
00376                     if(chk == chkCal) {
00377                         // Trama válida
00378                         memcpy((void *)RS485BufIn, (void *)rx_buffer, buffer_size+1);
00379                        
00380                         uint16_t tipo = RS485BufIn[2];  // Lee tipo de trama
00381                         if(tipo == TIPO_TRAMA_ESTADO_RS485) { // Si es trama estado del servo
00382 //                            pc.printf("Recibido Servo %d\r\n", RS485BufIn[3]);
00383                             st_trama_estado *p = (st_trama_estado *)&RS485BufIn[0];
00384                             // Procesa estado recibido desde el servo
00385                             procesaTramaServo(p);
00386                         } else if(tipo == TIPO_TRAMA_POSICION_RS485) { // Si es trama de posicion
00387 //                            pc.printf("Recibida Posicion Servo %d\r\n", RS485BufIn[3]);
00388                             st_trama_posicion *p = (st_trama_posicion *)&RS485BufIn[0];
00389                             // Procesa trama de posicion recibida desde los servos
00390                             procesaTramaPosicion(p);
00391                         } else if(tipo == TIPO_TRAMA_ESTADO_MANDO_RS485) { // Si es trama de estado mando
00392 //                            pc.printf("Recibido Mando %d\r\n", RS485BufIn[3]);
00393                             st_trama_estado_mando *p = (st_trama_estado_mando *)&RS485BufIn[0];
00394                             procesaTramaMando(p);
00395                         }
00396                     }
00397                 }
00398                 nc = 0;
00399                 inicio = false;   // Prepara para nueva trama
00400             }
00401             if(inicio==true)
00402                 nc++;
00403         } else {
00404             if(c == STX) {
00405                 nc = 1;
00406                 inicio = true;
00407                 rx_buffer[0] = c;
00408             }
00409         }
00410     }
00411     return;
00412 }
00413 
00414 
00415 //***********************************************
00416 // RUTINA DE ENVIO DE BUFFER POR RS485
00417 //***********************************************
00418 void SerialTxRS485(void)
00419 {
00420 static int nv = 0;
00421 static int t = 0;
00422 
00423     if(f_rs485_busy==true){
00424         if(tx_in == tx_out){    // Si ha finalizado
00425             wait(0.00015);
00426             RS485_DIR = 0;  // pone en RX, fin de transmision
00427             f_rs485_busy = false;   // borra flag
00428             led3 = 0;       // apaga led FIN TRANSMISION
00429 //            printf("bytes:%d nv:%d\r\n", tx_out, nv);
00430             nv=0;
00431             t=0;
00432         }
00433         else{
00434 //            serRS485.putc(tx_buffer[tx_out]);
00435 //            tx_out++;
00436             while ((tx_in != tx_out) && serRS485.writeable()) {  // si el buffer está vacio y hay bytes para enviar
00437                 serRS485.putc(tx_buffer[tx_out]);
00438                 tx_out = (tx_out + 1);
00439             }
00440             nv++;
00441         }
00442     }
00443     else{
00444         led3 = 0;       // apaga led FIN TRANSMISION        
00445     }
00446 }
00447 
00448 
00449 
00450 /********************************************************************
00451 // RUTINAS DE COMUNICACION POR MODBUS PARA PLCs
00452 ********************************************************************/
00453 
00454 //    uint8_t tram_modbus[]={0x01,0x10,0x70,0x12,0x00,0x04,0x08,0x00,0x00,0x40,0x40,0xCC,0xCD,0x40,0x40}; // pone a min=4.0 (40 80 00 00), max=5.01 (40 A0 51 EC)
00455     uint8_t tram_modbus[]={0x0A,0x04,0x00,0x00,0x00,0x0A};  //0x71,0x76 L,H
00456 
00457 
00458 
00459 /********************************************************************
00460 // CALCULO DE CRC PARA TRAMA MODBUS
00461 ********************************************************************/
00462 uint16_t crc16(uint8_t *buf, uint16_t len)
00463 {
00464     uint16_t crc = 0xFFFF;
00465 
00466     for (int pos = 0; pos < len; pos++) {
00467         crc ^= (uint16_t)buf[pos];          // XOR byte into least sig. byte of crc
00468 
00469         for (int i = 8; i != 0; i--) {    // Loop over each bit
00470             if ((crc & 0x0001) != 0) {      // If the LSB is set
00471                 crc >>= 1;                    // Shift right and XOR 0xA001
00472                 crc ^= 0xA001;
00473             } else                          // Else LSB is not set
00474                 crc >>= 1;                    // Just shift right
00475         }
00476     }
00477     // Note, this number has low and high bytes swapped, so use it accordingly (or swap bytes)
00478     return crc;
00479 }
00480 
00481 
00482 
00483 /********************************************************************
00484 // TEST PARA TRAMA MODBUS
00485 ********************************************************************/
00486 void test_modbus(void)
00487 {
00488      uint16_t crc;
00489 
00490     crc = crc16(tram_modbus, sizeof tram_modbus);
00491     printf("CRC=%04X\r\n", crc);
00492     
00493 }
00494 
00495 
00496     uint8_t MB_timones_w[]={0x01,0x10,0x70,0x12,0x00,0x04,0x08,0x00,0x00,0x40,0x40,0xD7,0x0A,0x40,0x73,0,0}; // pone a min=3.0 (40 40 00 00), max=3.8 (40 73 D7 0A)
00497 
00498 
00499 
00500 /*******************************************************************************
00501  * ENVIA TRAMA MODBUS RS485
00502  ******************************************************************************/
00503 void envia_trama_modbus(){
00504 
00505     if(f_rs485_busy)       // SI YA ESTA TRANSMITIENDO, FINALIZA
00506         return;
00507         
00508     if(f_actualiza_tim_trans==true || datos_servo.estado==DIR_RUMBO || datos_servo.estado == DIR_PILOTO || datos_servo.estado == DIR_MASTER){
00509 
00510         float hist = 0.2;
00511         long v_max = 8000;
00512         long v_min = 3000;
00513         
00514         float val_tb_min = ((float)map(datos_servo.timon_babor, 0, 100, v_min, v_max)/1000);
00515         float val_tb_max = ((float)map(datos_servo.timon_babor, 0, 100, v_min, v_max)/1000)+hist;
00516         float val_te_min = ((float)map(datos_servo.timon_babor, 0, 100, v_min, v_max)/1000);
00517         float val_te_max = ((float)map(datos_servo.timon_babor, 0, 100, v_min, v_max)/1000)+hist;
00518     
00519         uint8_t *p_tb_min = (uint8_t *)&val_tb_min;
00520         uint8_t *p_tb_max = (uint8_t *)&val_tb_max;
00521         uint8_t *p_te_min = (uint8_t *)&val_te_min;
00522         uint8_t *p_te_max = (uint8_t *)&val_te_max;
00523            
00524         // Prepara trama para envio
00525         uint8_t *pb = (uint8_t *)&tx_buffer;
00526         memcpy(pb, MB_timones_w, sizeof MB_timones_w);
00527         pb += 7;  // puntero a inicio de datos
00528         *pb++ = p_tb_min[1];
00529         *pb++ = p_tb_min[0];
00530         *pb++ = p_tb_min[3];
00531         *pb++ = p_tb_min[2];
00532         *pb++ = p_tb_max[1];
00533         *pb++ = p_tb_max[0];
00534         *pb++ = p_tb_max[3];
00535         *pb++ = p_tb_max[2];
00536     
00537         uint16_t crc = crc16((uint8_t *)&tx_buffer, (sizeof MB_timones_w)-2);
00538         *pb++ = (uint8_t)crc;
00539         *pb++ = (uint8_t)(crc>>8);
00540     
00541     //    PrintHex8((uint8_t *)&tx_buffer, sizeof(st_trama_estado));
00542     //    pc.printf("\r\n");
00543     
00544         RS485_DIR = 1;  // pone en TX
00545         tx_out = 0;
00546         tx_in = sizeof MB_timones_w;
00547         f_rs485_busy = true;     // TRANSMITIENDO
00548     
00549         led3 = 1;       // enciende led INICIO DE TRANSMISION
00550     
00551     //    printf("%dms envia estado: %d\r\n", millis()-tim_envio, datos_servo.estado);
00552     //    printf("est:%d pos:%d, est:%d pos:%d\r\n", datos_servo.estado, (int16_t)datos_servo.posicion_mando, datos_otro_servo.estado, (int16_t)posicion_otro_servo);
00553     
00554         tim_envio = millis();
00555         f_actualiza_tim_trans = false;     // borra flag para actualizar timones y transmision
00556     }  
00557 }
00558