Tom N / LidarSpi

This is a class which contains function to interface with the MLX75320

Dependents:   MLX75320_API

LidarSpi.cpp@6:748062f3de21, 2016-03-23 (annotated)

Committer:
TNU
Date:
Wed Mar 23 15:24:50 2016 +0000
Revision:
6:748062f3de21
Parent:
5:87e211a23654
Child:
7:c47612b25c77
Patch load: Fragments load now

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TNU 0:dfe498e03679 1 #include "mbed.h"
TNU 0:dfe498e03679 2 #include "LidarSpi.h"
TNU 0:dfe498e03679 3
TNU 0:dfe498e03679 4 //#include "ToolsClass.h"
TNU 0:dfe498e03679 5 #include <string.h>
TNU 0:dfe498e03679 6 #include "LidarSpi.h"
TNU 0:dfe498e03679 7 #include "MLX_BaseSPI.h"
TNU 6:748062f3de21 8 #include <inttypes.h>
TNU 0:dfe498e03679 9 #define SPIFREQ 8000
TNU 6:748062f3de21 10
TNU 3:9ed1d493c235 11 LidarSpi::LidarSpi(PinName mosi, PinName miso, PinName clk, PinName chipSelect, PinName dr, PinName rs, PinName tr, PinName smpl):device(mosi, miso, clk), chipS(chipSelect), dataReady(dr),resetPin(rs), trigger(tr), sampling(smpl)
TNU 0:dfe498e03679 12 {
TNU 0:dfe498e03679 13 //resetPin.write(1);
TNU 0:dfe498e03679 14 chipS.write(1);
TNU 0:dfe498e03679 15
TNU 0:dfe498e03679 16 //8 bit
TNU 0:dfe498e03679 17 //Mode 1: CPPOL=0, CLPHA=1 => Default of lidar, mbed side receiving is bad near 25Mhz
TNU 0:dfe498e03679 18 // -> mbed does not read first bit, this shifts the entire message on MISO 1 bit
TNU 0:dfe498e03679 19 //Mode 3: CLPOL=1, CLPHA=1 => Transmission on mbed side good, Lidar chip returns CRC errors on the succesful tranmissions
TNU 0:dfe498e03679 20 // -> Cant't send anything to lidar without changing lidar SPI mode
TNU 0:dfe498e03679 21 device.format(8,1); //8 bit, CLPOL=1, CLPHA=1
TNU 0:dfe498e03679 22 device.frequency(16000000);
TNU 0:dfe498e03679 23 }
TNU 0:dfe498e03679 24
TNU 0:dfe498e03679 25
TNU 0:dfe498e03679 26 int LidarSpi::BasicRead(){
TNU 0:dfe498e03679 27 return device.write(0x6B);
TNU 0:dfe498e03679 28
TNU 0:dfe498e03679 29 }
TNU 0:dfe498e03679 30
TNU 0:dfe498e03679 31
TNU 0:dfe498e03679 32 int LidarSpi::SpiSetting(long freq, int mode, Serial* pc){
TNU 0:dfe498e03679 33 int cPha= mode & 0x01;
TNU 0:dfe498e03679 34 int cPol=(mode>>1) & 0x01;
TNU 0:dfe498e03679 35 uint32_t val=0;
TNU 0:dfe498e03679 36 int res=0;
TNU 0:dfe498e03679 37 res=ReadReg(0x10E, &val,pc);
TNU 0:dfe498e03679 38 if(res<0) return -1;
TNU 0:dfe498e03679 39 val=val>>16;
TNU 0:dfe498e03679 40 val=val | (cPol<<5) | (cPha<<6);
TNU 0:dfe498e03679 41 wait_us(4);
TNU 0:dfe498e03679 42 res=WriteReg(0x10E, val, pc);
TNU 0:dfe498e03679 43 wait_us(4);
TNU 0:dfe498e03679 44 device.format(8, mode);
TNU 0:dfe498e03679 45 res=ReadReg(0x10E, &val,pc);
TNU 0:dfe498e03679 46
TNU 0:dfe498e03679 47 device.frequency(freq);
TNU 0:dfe498e03679 48 return res;
TNU 0:dfe498e03679 49 }
TNU 0:dfe498e03679 50
TNU 0:dfe498e03679 51
TNU 0:dfe498e03679 52 //int LidarSpi::BasicTransfer(uint8_t* rData, uint16_t rSz, const uint8_t* tData, uint16_t tSz, const event_callback_t callback){
TNU 0:dfe498e03679 53 // device.transfer(tData,tSz,rData,rSz,callback,event=SPI_EVENT_COMPLETE);
TNU 0:dfe498e03679 54 // //device.transfer((uint8_t*)_cmd, (2 + (DISPLAY_WIDTH / DISPLAY_BUFFER_TYPE_SIZE * sizeof(DISPLAY_BUFFER_TYPE))) , (uint8_t*)NULL, 0, _internalEventCallback, SPI_EVENT_COMPLETE) != 0)
TNU 0:dfe498e03679 55 // device.transfer();
TNU 0:dfe498e03679 56 // return 0;
TNU 0:dfe498e03679 57 //}
TNU 0:dfe498e03679 58
TNU 0:dfe498e03679 59
TNU 0:dfe498e03679 60 int LidarSpi::TxPacket(uint8_t* rData, uint16_t *rSz, uint8_t *tData, uint16_t tSz){
TNU 0:dfe498e03679 61 chipS=0;
TNU 0:dfe498e03679 62 int i =0;
TNU 0:dfe498e03679 63 for(i=0; i< tSz;i++){
TNU 0:dfe498e03679 64 *rData=device.write(*tData);
TNU 0:dfe498e03679 65 rData++;
TNU 0:dfe498e03679 66 tData++;
TNU 0:dfe498e03679 67 }
TNU 0:dfe498e03679 68 chipS=1;
TNU 0:dfe498e03679 69 *rSz=i;
TNU 0:dfe498e03679 70 return 0;
TNU 0:dfe498e03679 71 }
TNU 0:dfe498e03679 72
TNU 0:dfe498e03679 73 int LidarSpi::TxPacketWord(uint8_t* rData, uint16_t *rSz, uint8_t *tData, uint16_t tSz){
TNU 0:dfe498e03679 74 chipS=0;
TNU 0:dfe498e03679 75 int i =0;
TNU 0:dfe498e03679 76 uint16_t* recPoint=(uint16_t*)rData;
TNU 0:dfe498e03679 77 uint16_t* transPoint=(uint16_t*)tData;
TNU 0:dfe498e03679 78
TNU 0:dfe498e03679 79 for(i=0; i< tSz/2;i++){
TNU 0:dfe498e03679 80 *recPoint=device.write(*transPoint);
TNU 0:dfe498e03679 81 recPoint++;
TNU 0:dfe498e03679 82 transPoint++;
TNU 0:dfe498e03679 83 }
TNU 0:dfe498e03679 84 chipS=1;
TNU 0:dfe498e03679 85 *rSz=i*2;
TNU 0:dfe498e03679 86 return 0;
TNU 0:dfe498e03679 87 }
TNU 0:dfe498e03679 88
TNU 0:dfe498e03679 89
TNU 0:dfe498e03679 90
TNU 0:dfe498e03679 91 int LidarSpi::TxPacket(uint8_t* rData, uint16_t *rSz, uint8_t *tData, uint16_t tSz, Serial* pc){
TNU 0:dfe498e03679 92 chipS=0;
TNU 0:dfe498e03679 93 int i =0;
TNU 0:dfe498e03679 94 //pc->printf("Transmitting %d bytes...\n\r",tSz);
TNU 0:dfe498e03679 95 //pc->printf("Received: ");
TNU 0:dfe498e03679 96 for(i=0; i< tSz;i++){
TNU 0:dfe498e03679 97 //*(tData+i)=*(tData+i)+1; //<================Uncomment to write gibberish with wrong CRC
TNU 0:dfe498e03679 98 *(rData+i)=device.write(*(tData+i));
TNU 0:dfe498e03679 99 //pc->printf("%02X", *(rData+i));
TNU 0:dfe498e03679 100 //rData++;
TNU 0:dfe498e03679 101 //tData++;
TNU 0:dfe498e03679 102 }
TNU 0:dfe498e03679 103 //pc->printf("\n\r");
TNU 0:dfe498e03679 104 chipS=1;
TNU 0:dfe498e03679 105 *rSz=i;
TNU 0:dfe498e03679 106 return 0;
TNU 0:dfe498e03679 107 }
TNU 0:dfe498e03679 108
TNU 0:dfe498e03679 109 int LidarSpi::TxPacketSlow(uint8_t* rData, uint16_t *rSz, uint8_t *tData, uint16_t tSz, uint16_t usDelay){
TNU 0:dfe498e03679 110 int res=TxPacket(rData, rSz, tData, tSz);
TNU 0:dfe498e03679 111 wait_us(usDelay);
TNU 0:dfe498e03679 112 return res;
TNU 0:dfe498e03679 113 }
TNU 0:dfe498e03679 114
TNU 0:dfe498e03679 115 int LidarSpi::ReadReg(uint32_t reg, uint32_t *val)
TNU 0:dfe498e03679 116 {
TNU 0:dfe498e03679 117 int res;
TNU 0:dfe498e03679 118 uint16_t rSz;
TNU 0:dfe498e03679 119 PACK_SHORT rx[2], tx[2];
TNU 0:dfe498e03679 120
TNU 0:dfe498e03679 121 // Ensure all packets are all zeros to not send trash
TNU 0:dfe498e03679 122 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 123
TNU 0:dfe498e03679 124 // Encode the request and send it
TNU 0:dfe498e03679 125 res = MLX_ReqReadReg(tx, (uint32_t)reg);
TNU 0:dfe498e03679 126 if (res < 0)
TNU 0:dfe498e03679 127 return -1;
TNU 0:dfe498e03679 128
TNU 0:dfe498e03679 129 // tx[1].hdr = 0x0040;
TNU 0:dfe498e03679 130 // tx[1].crc = 0x106a;
TNU 0:dfe498e03679 131 res = MLX_EncodeStatusS(tx + 1, 0, 0);
TNU 0:dfe498e03679 132 if (res < 0)
TNU 0:dfe498e03679 133 return -2;
TNU 0:dfe498e03679 134
TNU 0:dfe498e03679 135 res = TxPacket((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx)/2);
TNU 0:dfe498e03679 136 wait_us(5);
TNU 0:dfe498e03679 137 res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2);
TNU 0:dfe498e03679 138 if (res < 0)
TNU 0:dfe498e03679 139 return -3;
TNU 0:dfe498e03679 140
TNU 0:dfe498e03679 141 // Decode the response packet with register value
TNU 0:dfe498e03679 142 res = MLX_DecodeResS(rx + 1, val);
TNU 0:dfe498e03679 143 if (res < 0)
TNU 0:dfe498e03679 144 return res;
TNU 0:dfe498e03679 145 //return sizeof(tx);
TNU 5:87e211a23654 146 wait_us(5);
TNU 0:dfe498e03679 147 return 0;
TNU 0:dfe498e03679 148 }
TNU 0:dfe498e03679 149
TNU 0:dfe498e03679 150 int LidarSpi::ReadReg(uint32_t reg, uint32_t *val, Serial* pc)
TNU 0:dfe498e03679 151 {
TNU 0:dfe498e03679 152 int res;
TNU 0:dfe498e03679 153 uint16_t rSz;
TNU 0:dfe498e03679 154 PACK_SHORT rx[2], tx[2];
TNU 0:dfe498e03679 155
TNU 0:dfe498e03679 156 // Ensure all packets are all zeros to not send trash
TNU 0:dfe498e03679 157 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 158
TNU 0:dfe498e03679 159 // Encode the request and send it
TNU 0:dfe498e03679 160 res = MLX_ReqReadReg(tx, (uint32_t)reg);
TNU 0:dfe498e03679 161 if (res < 0)
TNU 0:dfe498e03679 162 return -1;
TNU 0:dfe498e03679 163
TNU 0:dfe498e03679 164 // tx[1].hdr = 0x0040;
TNU 0:dfe498e03679 165 // tx[1].crc = 0x106a;
TNU 0:dfe498e03679 166 res = MLX_EncodeStatusS(tx + 1, 0, 0);
TNU 0:dfe498e03679 167 if (res < 0)
TNU 0:dfe498e03679 168 return -2;
TNU 0:dfe498e03679 169
TNU 0:dfe498e03679 170 res = TxPacket((uint8_t*)rx, &rSz, ((uint8_t*)tx), sizeof(tx)/2,pc);
TNU 0:dfe498e03679 171 wait_us(5);
TNU 0:dfe498e03679 172 res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2, pc);
TNU 0:dfe498e03679 173 if (res < 0)
TNU 0:dfe498e03679 174 return -3;
TNU 0:dfe498e03679 175
TNU 0:dfe498e03679 176 pc->printf("Read register request and response\n\r");
TNU 0:dfe498e03679 177
TNU 0:dfe498e03679 178 pc->printf("MOSI ReadRegRequest:\t");
TNU 0:dfe498e03679 179 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 180 uint8_t* pnt=(uint8_t*)tx;
TNU 0:dfe498e03679 181 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 182 }
TNU 0:dfe498e03679 183 pc->printf("\n\r");
TNU 0:dfe498e03679 184 pc->printf("MISO ReadRegRequest:\t");
TNU 0:dfe498e03679 185 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 186 uint8_t* pnt=(uint8_t*)rx;
TNU 0:dfe498e03679 187 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 188 }
TNU 0:dfe498e03679 189 pc->printf("\n\r");
TNU 0:dfe498e03679 190 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 191
TNU 0:dfe498e03679 192
TNU 0:dfe498e03679 193 pc->printf("MOSI ReadRegResponse:\t");
TNU 0:dfe498e03679 194 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 195 uint8_t* pnt=(uint8_t*)(tx+1);
TNU 0:dfe498e03679 196 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 197 }
TNU 0:dfe498e03679 198 pc->printf("\n\r");
TNU 0:dfe498e03679 199 pc->printf("MISO ReadRegResponse:\t");
TNU 0:dfe498e03679 200 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 201 uint8_t* pnt=(uint8_t*)(rx+1);
TNU 0:dfe498e03679 202 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 203 }
TNU 0:dfe498e03679 204 pc->printf("\n\r");
TNU 0:dfe498e03679 205 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 206
TNU 0:dfe498e03679 207 // Decode the response packet with register value
TNU 0:dfe498e03679 208 res = MLX_DecodeResS(rx + 1, val);
TNU 0:dfe498e03679 209 if (res < 0)
TNU 0:dfe498e03679 210 if(res==-6) {
TNU 0:dfe498e03679 211 uint8_t isser=0;
TNU 0:dfe498e03679 212 uint16_t err=0;
TNU 0:dfe498e03679 213 MLX_DecodeStatusS(rx+1,&isser, &err);
TNU 0:dfe498e03679 214 pc->printf("Not a valid ReadRequestResponse-> Decode as short status: Result: %d, IsError: %d, Error: %d\n\r", res,isser,err);
TNU 0:dfe498e03679 215 return res;
TNU 0:dfe498e03679 216 }
TNU 0:dfe498e03679 217 else return res;
TNU 0:dfe498e03679 218 //return sizeof(tx);
TNU 5:87e211a23654 219 wait_us(5);
TNU 0:dfe498e03679 220 return 0;
TNU 0:dfe498e03679 221 }
TNU 0:dfe498e03679 222
TNU 0:dfe498e03679 223 int LidarSpi::WriteReg(uint32_t reg, uint32_t val)
TNU 0:dfe498e03679 224 {
TNU 5:87e211a23654 225 int res=WriteRegSpeed(reg, val, START_DELAY);
TNU 5:87e211a23654 226 wait_us(5);
TNU 5:87e211a23654 227 return res;
TNU 5:87e211a23654 228
TNU 0:dfe498e03679 229 }
TNU 0:dfe498e03679 230
TNU 0:dfe498e03679 231 // Add a speed input so that we can control delay between packets
TNU 0:dfe498e03679 232 int LidarSpi::WriteRegSpeed(uint32_t reg, uint32_t val, uint16_t speed)
TNU 0:dfe498e03679 233 {
TNU 0:dfe498e03679 234 int res;
TNU 0:dfe498e03679 235 uint16_t rSz;
TNU 0:dfe498e03679 236 uint8_t iserr;
TNU 0:dfe498e03679 237 uint16_t error;
TNU 0:dfe498e03679 238 PACK_SHORT rx[2], tx[2];
TNU 0:dfe498e03679 239
TNU 0:dfe498e03679 240 // Ensure all packets are all zeros to not send trash
TNU 0:dfe498e03679 241 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 242
TNU 0:dfe498e03679 243 // Encode the request and send it
TNU 0:dfe498e03679 244 res = MLX_ReqWriteReg(tx, (uint32_t)reg, (uint32_t)val);
TNU 0:dfe498e03679 245 if (res < 0)
TNU 0:dfe498e03679 246 return res;
TNU 0:dfe498e03679 247
TNU 0:dfe498e03679 248 res = TxPacket((uint8_t*)rx, &rSz, ((uint8_t*)tx), sizeof(tx)/2);
TNU 0:dfe498e03679 249 wait_us(5);
TNU 0:dfe498e03679 250 res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2);
TNU 0:dfe498e03679 251 if (res < 0)
TNU 0:dfe498e03679 252 return res;
TNU 0:dfe498e03679 253
TNU 0:dfe498e03679 254
TNU 0:dfe498e03679 255
TNU 0:dfe498e03679 256 // Decode response (a status packet)
TNU 0:dfe498e03679 257 res = MLX_DecodeStatusS(rx + 1, &iserr, &error);
TNU 0:dfe498e03679 258 if (res < 0 || iserr)
TNU 0:dfe498e03679 259 return res;
TNU 0:dfe498e03679 260
TNU 0:dfe498e03679 261 return 0;
TNU 0:dfe498e03679 262 }
TNU 0:dfe498e03679 263
TNU 0:dfe498e03679 264 int LidarSpi::WriteReg(uint32_t reg, uint32_t val, Serial* pc)
TNU 0:dfe498e03679 265 {
TNU 5:87e211a23654 266 int res=WriteRegSpeed(reg, val, START_DELAY, pc);
TNU 5:87e211a23654 267 wait_us(5);
TNU 5:87e211a23654 268 return res;
TNU 0:dfe498e03679 269 }
TNU 0:dfe498e03679 270
TNU 0:dfe498e03679 271 // Add a speed input so that we can control delay between packets
TNU 0:dfe498e03679 272 int LidarSpi::WriteRegSpeed(uint32_t reg, uint32_t val, uint16_t speed, Serial* pc)
TNU 0:dfe498e03679 273 {
TNU 0:dfe498e03679 274 int res;
TNU 0:dfe498e03679 275 uint16_t rSz;
TNU 0:dfe498e03679 276 uint8_t iserr;
TNU 0:dfe498e03679 277 uint16_t error;
TNU 0:dfe498e03679 278 PACK_SHORT rx[2], tx[2];
TNU 0:dfe498e03679 279
TNU 0:dfe498e03679 280 // Ensure all packets are all zeros to not send trash
TNU 0:dfe498e03679 281 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 282
TNU 0:dfe498e03679 283 // Encode the request and send it
TNU 0:dfe498e03679 284 res = MLX_ReqWriteReg(tx, (uint32_t)reg, (uint32_t)val);
TNU 0:dfe498e03679 285 if (res < 0)
TNU 0:dfe498e03679 286 return res;
TNU 0:dfe498e03679 287
TNU 0:dfe498e03679 288 /*
TNU 0:dfe498e03679 289 res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), speed);
TNU 0:dfe498e03679 290 if (res < 0)
TNU 0:dfe498e03679 291 return res;
TNU 0:dfe498e03679 292 */
TNU 0:dfe498e03679 293 res = TxPacket((uint8_t*)rx, &rSz, ((uint8_t*)tx), sizeof(tx)/2,pc);
TNU 0:dfe498e03679 294 wait_us(5);
TNU 0:dfe498e03679 295 res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2, pc);
TNU 0:dfe498e03679 296 if (res < 0)
TNU 0:dfe498e03679 297 return -3;
TNU 0:dfe498e03679 298
TNU 0:dfe498e03679 299 //pc->printf("Write register request and response\n\r");
TNU 0:dfe498e03679 300
TNU 0:dfe498e03679 301 pc->printf("MOSI WriteRegRequest:\t 0x");
TNU 0:dfe498e03679 302 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 303 uint8_t* pnt=(uint8_t*)tx;
TNU 0:dfe498e03679 304 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 305 }
TNU 0:dfe498e03679 306 pc->printf("\n\r");
TNU 0:dfe498e03679 307 pc->printf("MISO WriteRegRequest:\t 0x");
TNU 0:dfe498e03679 308 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 309 uint8_t* pnt=(uint8_t*)rx;
TNU 0:dfe498e03679 310 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 311 }
TNU 0:dfe498e03679 312 pc->printf("\n\r");
TNU 0:dfe498e03679 313 pc->printf("-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 314
TNU 0:dfe498e03679 315
TNU 0:dfe498e03679 316 pc->printf("MOSI WriteRegResponse:\t 0x");
TNU 0:dfe498e03679 317 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 318 uint8_t* pnt=(uint8_t*)(tx+1);
TNU 0:dfe498e03679 319 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 320 }
TNU 0:dfe498e03679 321 pc->printf("\n\r");
TNU 0:dfe498e03679 322 pc->printf("MISO WriteRegResponse:\t 0x");
TNU 0:dfe498e03679 323 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 324 uint8_t* pnt=(uint8_t*)(rx+1);
TNU 0:dfe498e03679 325 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 326 }
TNU 0:dfe498e03679 327 pc->printf("\n\r");
TNU 0:dfe498e03679 328 pc->printf("==========================================================\n\r");
TNU 0:dfe498e03679 329
TNU 0:dfe498e03679 330
TNU 0:dfe498e03679 331
TNU 0:dfe498e03679 332 // Decode response (a status packet)
TNU 0:dfe498e03679 333 res = MLX_DecodeStatusS(rx + 1, &iserr, &error);
TNU 0:dfe498e03679 334 if (res < 0 || iserr){
TNU 0:dfe498e03679 335 pc->printf("Decode as short status: Result: %d, IsError: %d, Error: %d\n\r", res,iserr,error);
TNU 0:dfe498e03679 336 return res;}
TNU 0:dfe498e03679 337
TNU 0:dfe498e03679 338 return 0;
TNU 0:dfe498e03679 339 }
TNU 0:dfe498e03679 340 int LidarSpi::GetEchoes(Echo *ech, uint16_t maxN, uint16_t mode)
TNU 0:dfe498e03679 341 {
TNU 0:dfe498e03679 342 trigger.write(0);
TNU 0:dfe498e03679 343 int res, a, b;
TNU 0:dfe498e03679 344 uint16_t rSz;
TNU 0:dfe498e03679 345 uint32_t cnt;
TNU 0:dfe498e03679 346 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 347 PACK_LONG2 rxL, txL;
TNU 0:dfe498e03679 348 uint8_t iserr;
TNU 0:dfe498e03679 349 uint16_t err;
TNU 0:dfe498e03679 350 uint16_t idx = 0;
TNU 0:dfe498e03679 351 uint32_t val = 0;
TNU 0:dfe498e03679 352 uint16_t buf[MAXCH*4*BYTES_PER_ECH/2];
TNU 0:dfe498e03679 353
TNU 0:dfe498e03679 354 uint16_t * u16ptr ;
TNU 0:dfe498e03679 355
TNU 0:dfe498e03679 356
TNU 0:dfe498e03679 357 // Required buffer space
TNU 0:dfe498e03679 358 if (maxN < MAXECH)
TNU 0:dfe498e03679 359 return -1;
TNU 0:dfe498e03679 360
TNU 0:dfe498e03679 361 // Echo data is transmitted in 128 word payload => PACK_LONG2
TNU 0:dfe498e03679 362 // Each echo is X bytes, divide by payload size to get number of packets
TNU 0:dfe498e03679 363 const int nEchoes = MAXCH * 4;
TNU 0:dfe498e03679 364 const int nPack = nEchoes*BYTES_PER_ECH/MLX_LONG2_DATA_SZ;
TNU 0:dfe498e03679 365 const int WORDS_PER_ECH = BYTES_PER_ECH/2;
TNU 0:dfe498e03679 366
TNU 0:dfe498e03679 367 // Ensure transmitted packet is all zeros to not send trash
TNU 0:dfe498e03679 368 memset(&txL, 0, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 369 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 370
TNU 0:dfe498e03679 371 //res = MLXSPI::SetConfig(0); //debug
TNU 0:dfe498e03679 372
TNU 0:dfe498e03679 373 // Write 1 to PORT_ACQU register and then wait
TNU 0:dfe498e03679 374 res = ReadReg(0x146, &val); // PORT_ACQU
TNU 0:dfe498e03679 375 if (res < 0)
TNU 0:dfe498e03679 376 goto END;
TNU 0:dfe498e03679 377 val = (val >> 16) | 1;
TNU 0:dfe498e03679 378 trigger.write(1);
TNU 0:dfe498e03679 379 res = WriteReg(0x146, val); // PORT_ACQU
TNU 0:dfe498e03679 380 if (res < 0)
TNU 0:dfe498e03679 381 goto END;
TNU 0:dfe498e03679 382
TNU 0:dfe498e03679 383 // Wait till PORT_READY bit is set.
TNU 5:87e211a23654 384 trigger.write(1);
TNU 5:87e211a23654 385 // Wait till PORT_READY bit is set.
TNU 5:87e211a23654 386
TNU 0:dfe498e03679 387
TNU 5:87e211a23654 388 //res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 389 cnt = 0;
TNU 5:87e211a23654 390
TNU 5:87e211a23654 391 /*while (((val & 0x10000) >> 16 != 1) && (cnt < 2000)) {
TNU 0:dfe498e03679 392 wait_us(50);
TNU 0:dfe498e03679 393 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 394 cnt++;
TNU 5:87e211a23654 395 } */
TNU 5:87e211a23654 396 while((!dataReady.read())&& (cnt<2000)){
TNU 5:87e211a23654 397 wait_us(50);
TNU 5:87e211a23654 398 cnt++;
TNU 0:dfe498e03679 399 }
TNU 5:87e211a23654 400
TNU 5:87e211a23654 401
TNU 5:87e211a23654 402 res = ReadReg(470, &val); // PORT_READY
TNU 5:87e211a23654 403 val=val>>16;
TNU 6:748062f3de21 404 //pc->printf("PORT READY: %x\n\r",val);
TNU 0:dfe498e03679 405 trigger.write(0);
TNU 0:dfe498e03679 406 // Encode the request and send it
TNU 0:dfe498e03679 407 res = MLX_ReqReadEch(tx);
TNU 0:dfe498e03679 408 if (res < 0)
TNU 0:dfe498e03679 409 goto END;
TNU 0:dfe498e03679 410
TNU 0:dfe498e03679 411 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 412 //----SHORT PACKET EXCHANGE--------
TNU 0:dfe498e03679 413 res = TxPacketSlow((uint8*)rx, &rSz, (uint8*)tx, sizeof(tx), 4);
TNU 0:dfe498e03679 414 if (res < 0)
TNU 0:dfe498e03679 415 goto END;
TNU 0:dfe498e03679 416 //----PACKET EXCHANGE
TNU 0:dfe498e03679 417
TNU 0:dfe498e03679 418 // Optional status decoding
TNU 0:dfe498e03679 419 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 0:dfe498e03679 420 if (res < 0 || iserr)
TNU 0:dfe498e03679 421 goto END;
TNU 0:dfe498e03679 422
TNU 0:dfe498e03679 423 // Temporary pointer to uint16 data, for simplicity purpose
TNU 0:dfe498e03679 424 u16ptr = (uint16*)rxL.data;
TNU 0:dfe498e03679 425
TNU 0:dfe498e03679 426
TNU 0:dfe498e03679 427 res = MLX_EncodeStatusL2(&txL, 0, 0); //Create echo status packet
TNU 0:dfe498e03679 428 if (res < 0){
TNU 0:dfe498e03679 429 res = -7;
TNU 0:dfe498e03679 430 goto END;
TNU 0:dfe498e03679 431 }
TNU 0:dfe498e03679 432 for (a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 433 {
TNU 0:dfe498e03679 434
TNU 0:dfe498e03679 435 wait_us(10);
TNU 0:dfe498e03679 436 //Tools::Wait(10);
TNU 0:dfe498e03679 437 // Clock the remaining long packets
TNU 0:dfe498e03679 438 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 439 //----LONG PACKET EXCHANGE--------
TNU 0:dfe498e03679 440 res=TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 441 if (res < 0)
TNU 0:dfe498e03679 442 goto END;
TNU 0:dfe498e03679 443
TNU 0:dfe498e03679 444 // Decode the long responses, then extract data values
TNU 0:dfe498e03679 445 res = MLX_DecodeResL2(&rxL);
TNU 0:dfe498e03679 446 if (res < 0)
TNU 0:dfe498e03679 447 goto END;
TNU 0:dfe498e03679 448
TNU 0:dfe498e03679 449 // Gather all of the echo data in a buffer.
TNU 0:dfe498e03679 450 //for (b = 0; b < (MLX_LONG2_DATA_SZ / 2); ++b)
TNU 0:dfe498e03679 451 for (b = 0; b < (128 / 2); ++b)
TNU 0:dfe498e03679 452 {
TNU 0:dfe498e03679 453 //buf[a*(MLX_LONG2_DATA_SZ / 2) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte
TNU 0:dfe498e03679 454 buf[a*(128 / 2) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte
TNU 0:dfe498e03679 455 }
TNU 0:dfe498e03679 456 }
TNU 0:dfe498e03679 457
TNU 0:dfe498e03679 458 // Do something with the data... decode an echo, manage the empty echo case
TNU 0:dfe498e03679 459 for (b = 0; b < nEchoes; ++b) {
TNU 0:dfe498e03679 460
TNU 0:dfe498e03679 461 ech[idx].mDistance = buf[b*WORDS_PER_ECH + 1] << 16 | buf[b*WORDS_PER_ECH];
TNU 0:dfe498e03679 462 ech[idx].mAmplitude = buf[b*WORDS_PER_ECH + 3] << 16 | buf[b*WORDS_PER_ECH +2];
TNU 0:dfe498e03679 463 ech[idx].mBase = buf[b*WORDS_PER_ECH + 5] << 16 | buf[b*WORDS_PER_ECH + 4];
TNU 0:dfe498e03679 464 ech[idx].mMaxIndex = buf[b*WORDS_PER_ECH + 6];
TNU 0:dfe498e03679 465 ech[idx].mChannelIndex = (uint8) buf[b*WORDS_PER_ECH + 7];
TNU 0:dfe498e03679 466 ech[idx].mValid = buf[b*WORDS_PER_ECH + 7] >>8;
TNU 0:dfe498e03679 467 ech[idx].mAmplitudeLowScale = buf[b*WORDS_PER_ECH + 9] | buf[b*WORDS_PER_ECH + 8];
TNU 0:dfe498e03679 468 ech[idx].mSaturationWidth = buf[b*WORDS_PER_ECH + 11] | buf[b*WORDS_PER_ECH + 10];
TNU 0:dfe498e03679 469
TNU 0:dfe498e03679 470 ++idx;
TNU 0:dfe498e03679 471 }
TNU 0:dfe498e03679 472
TNU 0:dfe498e03679 473 res = idx;
TNU 0:dfe498e03679 474
TNU 0:dfe498e03679 475 END:
TNU 0:dfe498e03679 476
TNU 0:dfe498e03679 477 return res;
TNU 0:dfe498e03679 478 }
TNU 0:dfe498e03679 479
TNU 0:dfe498e03679 480
TNU 0:dfe498e03679 481 int LidarSpi::GetEchoes(Echo *ech, uint16_t maxN, uint16_t mode, Serial* pc)
TNU 0:dfe498e03679 482 {
TNU 0:dfe498e03679 483 trigger.write(0);
TNU 0:dfe498e03679 484 int res, a, b;
TNU 0:dfe498e03679 485 uint16_t rSz;
TNU 0:dfe498e03679 486 uint32_t cnt;
TNU 0:dfe498e03679 487 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 488 PACK_LONG2 rxL, txL;
TNU 0:dfe498e03679 489 uint8_t iserr;
TNU 0:dfe498e03679 490 uint16_t err;
TNU 0:dfe498e03679 491 uint16_t idx = 0;
TNU 0:dfe498e03679 492 uint32_t val = 0;
TNU 0:dfe498e03679 493 uint16_t buf[MAXCH*4*BYTES_PER_ECH/2];
TNU 0:dfe498e03679 494 memset(buf, 0, MAXCH*4*BYTES_PER_ECH/2);
TNU 0:dfe498e03679 495 uint16_t * u16ptr ;
TNU 0:dfe498e03679 496
TNU 0:dfe498e03679 497
TNU 0:dfe498e03679 498 // Required buffer space
TNU 0:dfe498e03679 499 if (maxN < MAXECH){
TNU 0:dfe498e03679 500 pc->printf("maxN too small\n\r");
TNU 0:dfe498e03679 501 return -1;
TNU 0:dfe498e03679 502 }
TNU 0:dfe498e03679 503
TNU 0:dfe498e03679 504 // Echo data is transmitted in 128 word payload => PACK_LONG2
TNU 0:dfe498e03679 505 // Each echo is X bytes, divide by payload size to get number of packets
TNU 0:dfe498e03679 506 const int nEchoes = MAXCH * 4;
TNU 0:dfe498e03679 507 const int nPack = nEchoes*BYTES_PER_ECH/MLX_LONG2_DATA_SZ;
TNU 0:dfe498e03679 508 const int WORDS_PER_ECH = BYTES_PER_ECH/2;
TNU 0:dfe498e03679 509
TNU 0:dfe498e03679 510 // Ensure transmitted packet is all zeros to not send trash
TNU 0:dfe498e03679 511 memset(&txL, 0, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 512 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 513
TNU 0:dfe498e03679 514 //res = MLXSPI::SetConfig(0); //debug
TNU 0:dfe498e03679 515
TNU 0:dfe498e03679 516 // Write 1 to PORT_ACQU register and then wait
TNU 0:dfe498e03679 517 pc->printf("\tRead PORT_ACQU\n\r");
TNU 0:dfe498e03679 518 res = ReadReg(0x146, &val); // PORT_ACQU
TNU 0:dfe498e03679 519 if (res < 0){
TNU 0:dfe498e03679 520 pc->printf("ReadReg Error\n\r");
TNU 0:dfe498e03679 521 goto END;}
TNU 0:dfe498e03679 522 val = (val >> 16) | 1;
TNU 0:dfe498e03679 523 pc->printf("\tWrite 1 to PORT_ACQU\n\r");
TNU 0:dfe498e03679 524 trigger.write(1);
TNU 0:dfe498e03679 525 res = WriteReg(0x146, val); // PORT_ACQU
TNU 0:dfe498e03679 526 if (res < 0){
TNU 0:dfe498e03679 527 pc->printf("WriteReg Error\n\r");
TNU 0:dfe498e03679 528 goto END;}
TNU 0:dfe498e03679 529
TNU 0:dfe498e03679 530 // Wait till PORT_READY bit is set.
TNU 0:dfe498e03679 531
TNU 0:dfe498e03679 532 pc->printf("\tWait for PORT_READY bit\n\r");
TNU 0:dfe498e03679 533 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 534 while(res<0){
TNU 0:dfe498e03679 535 pc->printf("Error: reread state of ready PIN\n\r");
TNU 0:dfe498e03679 536 wait_us(4);
TNU 0:dfe498e03679 537 res=ReadReg(470, &val);
TNU 0:dfe498e03679 538 }
TNU 0:dfe498e03679 539 cnt = 0;
TNU 5:87e211a23654 540 trigger.write(1);
TNU 5:87e211a23654 541 // Wait till PORT_READY bit is set.
TNU 5:87e211a23654 542
TNU 5:87e211a23654 543
TNU 5:87e211a23654 544 //res = ReadReg(470, &val); // PORT_READY
TNU 5:87e211a23654 545 cnt = 0;
TNU 5:87e211a23654 546
TNU 5:87e211a23654 547 /*while (((val & 0x10000) >> 16 != 1) && (cnt < 2000)) {
TNU 0:dfe498e03679 548 wait_us(50);
TNU 0:dfe498e03679 549 res = ReadReg(470, &val); // PORT_READY
TNU 5:87e211a23654 550 cnt++;
TNU 5:87e211a23654 551 } */
TNU 5:87e211a23654 552 while((!dataReady.read())&& (cnt<2000)){
TNU 5:87e211a23654 553 wait_us(50);
TNU 0:dfe498e03679 554 cnt++;
TNU 0:dfe498e03679 555 }
TNU 5:87e211a23654 556
TNU 5:87e211a23654 557
TNU 5:87e211a23654 558 res = ReadReg(470, &val); // PORT_READY
TNU 5:87e211a23654 559 val=val>>16;
TNU 5:87e211a23654 560 pc->printf("PORT READY: %x\n\r",val);
TNU 0:dfe498e03679 561 pc->printf("Counter: %d\n\r", cnt);
TNU 0:dfe498e03679 562
TNU 0:dfe498e03679 563 // Encode the request and send it
TNU 0:dfe498e03679 564 res = MLX_ReqReadEch(tx);
TNU 0:dfe498e03679 565 if (res < 0){
TNU 0:dfe498e03679 566 pc->printf("ReqreadEch error\n\r");
TNU 0:dfe498e03679 567 goto END;}
TNU 0:dfe498e03679 568
TNU 0:dfe498e03679 569 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 570 //----SHORT PACKET EXCHANGE--------
TNU 0:dfe498e03679 571 pc->printf("\tSend ReqReadEch\n\r");
TNU 0:dfe498e03679 572 res = TxPacketSlow((uint8*)rx, &rSz, (uint8*)tx, sizeof(tx), 4);
TNU 0:dfe498e03679 573 if (res < 0){
TNU 0:dfe498e03679 574 pc->printf("txPacketSlow Error\n\r");
TNU 0:dfe498e03679 575 goto END;}
TNU 0:dfe498e03679 576 /*
TNU 0:dfe498e03679 577 pc->printf("\tFirmware read request - processed\n\r");
TNU 0:dfe498e03679 578 pc->printf("Firmware readRequest MOSI: \t");
TNU 0:dfe498e03679 579 for(int i =0; i<sizeof(tx);i++) {
TNU 0:dfe498e03679 580 uint8_t* pnt=(uint8_t*)(&tx);
TNU 0:dfe498e03679 581 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 582 }
TNU 0:dfe498e03679 583 pc->printf("\n\r");
TNU 0:dfe498e03679 584 pc->printf("Firmware readRequest MISO: \t");
TNU 0:dfe498e03679 585 for(int i =0; i<sizeof(rx);i++) {
TNU 0:dfe498e03679 586 uint8_t* pnt=(uint8_t*)(&rx);
TNU 0:dfe498e03679 587 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 588 }
TNU 0:dfe498e03679 589 pc->printf("\n\r");
TNU 0:dfe498e03679 590 pc->printf("-- -- -- -- -- -- --\n\r");*/
TNU 0:dfe498e03679 591
TNU 0:dfe498e03679 592 //----PACKET EXCHANGE
TNU 0:dfe498e03679 593
TNU 0:dfe498e03679 594 // Optional status decoding
TNU 0:dfe498e03679 595 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 0:dfe498e03679 596 if (res < 0 || iserr){
TNU 0:dfe498e03679 597 pc->printf("Short status decode: Iss err?\n\r");
TNU 0:dfe498e03679 598 goto END;}
TNU 0:dfe498e03679 599
TNU 0:dfe498e03679 600 // Temporary pointer to uint16 data, for simplicity purpose
TNU 0:dfe498e03679 601 u16ptr = (uint16*)rxL.data;
TNU 0:dfe498e03679 602 PACK_LONG2 prevRX;
TNU 0:dfe498e03679 603 PACK_LONG2 prevTX;
TNU 0:dfe498e03679 604 res = MLX_EncodeStatusL2(&txL, 0, 0); //Create echo status packet
TNU 0:dfe498e03679 605 memset(&prevRX,0,sizeof(prevRX));
TNU 0:dfe498e03679 606 memset(&prevTX,0,sizeof(prevTX));
TNU 0:dfe498e03679 607 pc->printf("Value of npack:%d \n\r", nPack);
TNU 0:dfe498e03679 608 for (a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 609 {
TNU 0:dfe498e03679 610
TNU 0:dfe498e03679 611 if (res < 0){
TNU 0:dfe498e03679 612 res = -7;
TNU 0:dfe498e03679 613 pc->printf("Problem creating echo status\n\r");
TNU 0:dfe498e03679 614 goto END;
TNU 0:dfe498e03679 615 }
TNU 0:dfe498e03679 616 wait_us(10);
TNU 0:dfe498e03679 617 //Tools::Wait(10);
TNU 0:dfe498e03679 618 // Clock the remaining long packets
TNU 0:dfe498e03679 619 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 620 //----LONG PACKET EXCHANGE--------
TNU 0:dfe498e03679 621 res=TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 622 if (res < 0){
TNU 0:dfe498e03679 623 pc->printf("Packet #%d => txPacket_long error\n\r",a);
TNU 0:dfe498e03679 624 goto END;}
TNU 0:dfe498e03679 625
TNU 0:dfe498e03679 626
TNU 0:dfe498e03679 627 // Decode the long responses, then extract data values
TNU 0:dfe498e03679 628 res = MLX_DecodeResL2(&rxL);
TNU 1:e3ace426cee5 629 if ((res < 0)){
TNU 0:dfe498e03679 630
TNU 0:dfe498e03679 631 pc->printf("\n\r");
TNU 0:dfe498e03679 632 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 633 pc->printf("Packet #%d => Decode long response error \n\r", a);
TNU 0:dfe498e03679 634
TNU 0:dfe498e03679 635 pc->printf("TXLONG MOSI Response: ");
TNU 0:dfe498e03679 636 for(int i=0;i<(sizeof(PACK_LONG2));i++){
TNU 0:dfe498e03679 637 uint8_t* pnt=(uint8_t*)(&txL);
TNU 0:dfe498e03679 638 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 639 }
TNU 0:dfe498e03679 640 pc->printf("\n\r");
TNU 0:dfe498e03679 641 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 642 pc->printf("TXLONG MISO Response: ");
TNU 0:dfe498e03679 643 for(int i=0;i<(sizeof(PACK_LONG2));i++){
TNU 0:dfe498e03679 644 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 0:dfe498e03679 645 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 646
TNU 0:dfe498e03679 647 }
TNU 0:dfe498e03679 648 pc->printf("\n\r");
TNU 0:dfe498e03679 649 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 650 pc->printf("Packet #%d => Decode long response error \n\r", a);
TNU 0:dfe498e03679 651 goto END;}
TNU 0:dfe498e03679 652
TNU 0:dfe498e03679 653 // Gather all of the echo data in a buffer.
TNU 0:dfe498e03679 654 //for (b = 0; b < (MLX_LONG2_DATA_SZ / 2); ++b)
TNU 1:e3ace426cee5 655 for (b = 0; b < (128); ++b)
TNU 0:dfe498e03679 656 {
TNU 0:dfe498e03679 657 //buf[a*(MLX_LONG2_DATA_SZ / 2) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte
TNU 1:e3ace426cee5 658 buf[a*(128) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte
TNU 0:dfe498e03679 659 }
TNU 0:dfe498e03679 660 //prevTX=txL;
TNU 0:dfe498e03679 661 //prevRX=rxL;
TNU 0:dfe498e03679 662 }
TNU 0:dfe498e03679 663
TNU 0:dfe498e03679 664 // Do something with the data... decode an echo, manage the empty echo case
TNU 0:dfe498e03679 665 for (b = 0; b < nEchoes; ++b) {
TNU 0:dfe498e03679 666
TNU 0:dfe498e03679 667 ech[idx].mDistance = buf[b*WORDS_PER_ECH + 1] << 16 | buf[b*WORDS_PER_ECH];
TNU 0:dfe498e03679 668 ech[idx].mAmplitude = buf[b*WORDS_PER_ECH + 3] << 16 | buf[b*WORDS_PER_ECH +2];
TNU 0:dfe498e03679 669 ech[idx].mBase = buf[b*WORDS_PER_ECH + 5] << 16 | buf[b*WORDS_PER_ECH + 4];
TNU 0:dfe498e03679 670 ech[idx].mMaxIndex = buf[b*WORDS_PER_ECH + 6];
TNU 0:dfe498e03679 671 ech[idx].mChannelIndex = (uint8) buf[b*WORDS_PER_ECH + 7];
TNU 0:dfe498e03679 672 ech[idx].mValid = buf[b*WORDS_PER_ECH + 7] >>8;
TNU 0:dfe498e03679 673 ech[idx].mAmplitudeLowScale = buf[b*WORDS_PER_ECH + 9] | buf[b*WORDS_PER_ECH + 8];
TNU 0:dfe498e03679 674 ech[idx].mSaturationWidth = buf[b*WORDS_PER_ECH + 11] | buf[b*WORDS_PER_ECH + 10];
TNU 0:dfe498e03679 675
TNU 0:dfe498e03679 676 ++idx;
TNU 0:dfe498e03679 677 }
TNU 0:dfe498e03679 678
TNU 0:dfe498e03679 679 res = idx;
TNU 0:dfe498e03679 680 trigger.write(0);
TNU 0:dfe498e03679 681
TNU 0:dfe498e03679 682 END:
TNU 0:dfe498e03679 683 trigger.write(0);
TNU 0:dfe498e03679 684 return res;
TNU 0:dfe498e03679 685 }
TNU 0:dfe498e03679 686
TNU 0:dfe498e03679 687
TNU 0:dfe498e03679 688
TNU 0:dfe498e03679 689
TNU 0:dfe498e03679 690 int LidarSpi::GetTrace ( uint16_t *buf, uint16_t maxN, uint16_t nSam, uint16_t idx, Serial* pc){
TNU 3:9ed1d493c235 691 trigger.write(0);
TNU 0:dfe498e03679 692 int res, a, b, i;
TNU 0:dfe498e03679 693 uint32_t cnt;
TNU 0:dfe498e03679 694 uint16_t rSz;
TNU 0:dfe498e03679 695 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 696 PACK_LONG1 rxL, txL;
TNU 0:dfe498e03679 697 uint8_t iserr;
TNU 0:dfe498e03679 698 uint16_t err;
TNU 0:dfe498e03679 699 uint32_t val = 0;
TNU 0:dfe498e03679 700
TNU 0:dfe498e03679 701 uint16_t * u16ptr;
TNU 0:dfe498e03679 702 pc->printf("Buffer space required: %d\n\r", MAXTRCLEN);
TNU 0:dfe498e03679 703 // Required buffer space
TNU 0:dfe498e03679 704 if (maxN < MAXTRCLEN){
TNU 0:dfe498e03679 705 pc-printf("NOT ENOUGH BUFFER SPACEn\r");
TNU 0:dfe498e03679 706 return -1;}
TNU 0:dfe498e03679 707
TNU 0:dfe498e03679 708 // Divide by payload size to get number of packets
TNU 0:dfe498e03679 709 // const int nPack = MAXTRCLEN / MLX_LONG_DATA_SZ;
TNU 0:dfe498e03679 710 // WTA: change nPack to a variable, initially 64
TNU 0:dfe498e03679 711 int nPack = MAXTRCLEN / MLX_LONG1_DATA_SZ;
TNU 0:dfe498e03679 712 pc->printf("npack: %d", nPack);
TNU 0:dfe498e03679 713 // Ensure transmitted packet is all zeros to not send trash
TNU 0:dfe498e03679 714 memset(&txL, 0, sizeof(PACK_LONG1));
TNU 0:dfe498e03679 715
TNU 0:dfe498e03679 716 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 717 // memset(rx, 0, sizeof(rx));
TNU 0:dfe498e03679 718
TNU 0:dfe498e03679 719
TNU 0:dfe498e03679 720 res = ReadReg(336, &val); // PORT_CHSEL
TNU 0:dfe498e03679 721 if (res < 0)
TNU 0:dfe498e03679 722 goto END;
TNU 0:dfe498e03679 723 val >>= 16;
TNU 0:dfe498e03679 724 pc->printf("chsel = %d\n", val);
TNU 0:dfe498e03679 725 cnt = 0;
TNU 0:dfe498e03679 726 // Count how many channels are selected
TNU 0:dfe498e03679 727 for (i = 0; i < 16; i++) {
TNU 0:dfe498e03679 728 if (val & 0x1)
TNU 0:dfe498e03679 729 cnt++;
TNU 0:dfe498e03679 730 val >>= 1;
TNU 0:dfe498e03679 731 }
TNU 3:9ed1d493c235 732
TNU 0:dfe498e03679 733 nPack *= cnt;
TNU 0:dfe498e03679 734 nPack /= 16;
TNU 3:9ed1d493c235 735
TNU 0:dfe498e03679 736 res = ReadReg(332, &val); // PORT_OVR_ACCUM_ACQ_OVR
TNU 0:dfe498e03679 737 // pc->printf("PORT_OVR = %d\n", (val >> 16));
TNU 0:dfe498e03679 738 val = (val >> 16) & 3; // Get bits 0 and 1
TNU 0:dfe498e03679 739
TNU 0:dfe498e03679 740 if (res < 0){
TNU 0:dfe498e03679 741 pc->printf("ReadReg Error1\n\r");
TNU 0:dfe498e03679 742 goto END;}
TNU 0:dfe498e03679 743
TNU 0:dfe498e03679 744 if (val == 0){ //00 = 1
TNU 0:dfe498e03679 745 nPack /= 4;
TNU 0:dfe498e03679 746 }
TNU 0:dfe498e03679 747 else if (val == 1){ //01 = 2
TNU 0:dfe498e03679 748 nPack /= 4;
TNU 0:dfe498e03679 749 }
TNU 0:dfe498e03679 750 else if (val == 2){ //10 = 4
TNU 0:dfe498e03679 751 nPack /= 2;
TNU 0:dfe498e03679 752 }
TNU 0:dfe498e03679 753 else if (val == 3){ //11 = 8
TNU 0:dfe498e03679 754 nPack /= 1;
TNU 0:dfe498e03679 755 }
TNU 0:dfe498e03679 756 else {
TNU 0:dfe498e03679 757 pc->printf("GetTrace: bad value\n");
TNU 0:dfe498e03679 758 }
TNU 0:dfe498e03679 759
TNU 3:9ed1d493c235 760
TNU 0:dfe498e03679 761 // Write 1 to PORT_ACQU register and then wait
TNU 0:dfe498e03679 762 res = ReadReg(0x146, &val); // PORT_ACQU
TNU 0:dfe498e03679 763 if (res < 0){
TNU 0:dfe498e03679 764 pc->printf("ReadReg Error2\n\r");
TNU 0:dfe498e03679 765 goto END;}
TNU 0:dfe498e03679 766 val = (val>>16) | 1;
TNU 5:87e211a23654 767
TNU 0:dfe498e03679 768 res = WriteReg(0x146, val); // PORT_ACQU
TNU 0:dfe498e03679 769 if (res < 0){
TNU 0:dfe498e03679 770 pc->printf("WriteReg Error3\n\r");
TNU 0:dfe498e03679 771 goto END;}
TNU 5:87e211a23654 772 trigger.write(1);
TNU 0:dfe498e03679 773 // Wait till PORT_READY bit is set.
TNU 3:9ed1d493c235 774
TNU 3:9ed1d493c235 775
TNU 5:87e211a23654 776 //res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 777 cnt = 0;
TNU 5:87e211a23654 778
TNU 5:87e211a23654 779 /*while (((val & 0x10000) >> 16 != 1) && (cnt < 2000)) {
TNU 0:dfe498e03679 780 wait_us(50);
TNU 0:dfe498e03679 781 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 782 cnt++;
TNU 5:87e211a23654 783 } */
TNU 5:87e211a23654 784 while((!dataReady.read())&& (cnt<2000)){
TNU 3:9ed1d493c235 785 wait_us(50);
TNU 3:9ed1d493c235 786 cnt++;
TNU 0:dfe498e03679 787 }
TNU 3:9ed1d493c235 788
TNU 3:9ed1d493c235 789
TNU 5:87e211a23654 790 res = ReadReg(470, &val); // PORT_READY
TNU 5:87e211a23654 791 val=val>>16;
TNU 5:87e211a23654 792 pc->printf("PORT READY: %x\n\r",val);
TNU 5:87e211a23654 793
TNU 3:9ed1d493c235 794 trigger.write(0);
TNU 3:9ed1d493c235 795 pc-printf("Count: %d\n\r", cnt);
TNU 0:dfe498e03679 796
TNU 0:dfe498e03679 797 // Encode the request and send it
TNU 0:dfe498e03679 798 res = MLX_ReqReadTrc(tx);
TNU 0:dfe498e03679 799 if (res < 0)
TNU 0:dfe498e03679 800 goto END;
TNU 0:dfe498e03679 801
TNU 0:dfe498e03679 802
TNU 0:dfe498e03679 803 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 804 //----SHORT PACKET EXCHANGE--------
TNU 0:dfe498e03679 805 res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), 0);
TNU 0:dfe498e03679 806 if (res < 0)
TNU 0:dfe498e03679 807 goto END;
TNU 0:dfe498e03679 808 //----PACKET EXCHANGE
TNU 0:dfe498e03679 809 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 810
TNU 0:dfe498e03679 811
TNU 0:dfe498e03679 812 // Optional status decoding
TNU 0:dfe498e03679 813 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 0:dfe498e03679 814 if (res < 0 || iserr)
TNU 0:dfe498e03679 815 goto END;
TNU 0:dfe498e03679 816
TNU 0:dfe498e03679 817
TNU 0:dfe498e03679 818 // Temporary pointer to uint16 data, for simplicity purpose
TNU 0:dfe498e03679 819 u16ptr = (uint16_t*)rxL.data;
TNU 0:dfe498e03679 820 //device.format(16,1);
TNU 3:9ed1d493c235 821 //trigger.write(1);
TNU 0:dfe498e03679 822 for (a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 823 {
TNU 0:dfe498e03679 824 res = MLX_EncodeStatusL1(&txL, 0, 0);
TNU 0:dfe498e03679 825 if (res < 0){
TNU 0:dfe498e03679 826 res = -7;
TNU 0:dfe498e03679 827 goto END;
TNU 0:dfe498e03679 828 }
TNU 0:dfe498e03679 829
TNU 0:dfe498e03679 830 //Tools::Wait(10);
TNU 0:dfe498e03679 831 wait_us(10);
TNU 0:dfe498e03679 832
TNU 0:dfe498e03679 833 // Clock the remaining long packets
TNU 0:dfe498e03679 834 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG1));
TNU 0:dfe498e03679 835 // pc->printf("\nRXL%d = 0x%02x%02x%02x...[x294 bytes]...%02x%02x%02x\n", a + 1, rxL.buf[0], rxL.buf[1], rxL.buf[2], rxL.buf[297], rxL.buf[298], rxL.buf[299]);
TNU 0:dfe498e03679 836 // pc->printf("\nRXL%d = 0x%02x%02x%02x%02x%02x%02x\n", a + 1, rxL.buf[2], rxL.buf[3], rxL.buf[4], rxL.buf[5], rxL.buf[6], rxL.buf[7]);
TNU 0:dfe498e03679 837 if (res < 0){
TNU 0:dfe498e03679 838 res = -8;
TNU 0:dfe498e03679 839 goto END;
TNU 0:dfe498e03679 840 }
TNU 0:dfe498e03679 841
TNU 0:dfe498e03679 842 // Decode the long responses, then extract data values
TNU 0:dfe498e03679 843 res = MLX_DecodeResL1(&rxL);
TNU 0:dfe498e03679 844 if ((res < 0)){
TNU 0:dfe498e03679 845 pc->printf("LONG READ ERROR: Stopped at the %d long message\n", a);
TNU 0:dfe498e03679 846 pc->printf("TXLONG MOSI Response: ");
TNU 0:dfe498e03679 847 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 848 uint8_t* pnt=(uint8_t*)(&txL);
TNU 0:dfe498e03679 849 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 850 }
TNU 0:dfe498e03679 851 pc->printf("\n\r");
TNU 0:dfe498e03679 852 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 853 pc->printf("TXLONG MISO Response: ");
TNU 0:dfe498e03679 854 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 855 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 0:dfe498e03679 856 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 857
TNU 0:dfe498e03679 858 }
TNU 0:dfe498e03679 859 pc->printf("\n\r");
TNU 0:dfe498e03679 860 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 861 //printf("last RXL = \n");
TNU 0:dfe498e03679 862 //for (i = 0; i < 300; i++) {
TNU 0:dfe498e03679 863 // pc->printf("%02x", rxL.buf[i]);
TNU 0:dfe498e03679 864 // if (((i + 1) % 8) == 0)
TNU 0:dfe498e03679 865 // pc->printf("\n");
TNU 0:dfe498e03679 866 //}
TNU 0:dfe498e03679 867 //printf("\n");
TNU 0:dfe498e03679 868 res = -9;
TNU 0:dfe498e03679 869 goto END;
TNU 0:dfe498e03679 870 }
TNU 0:dfe498e03679 871
TNU 0:dfe498e03679 872 // Copy the returned data into the user buffer
TNU 0:dfe498e03679 873 // WTA: only half the MLX_LONG_DATA_SZ because we copy 2 bytes at a time
TNU 0:dfe498e03679 874 for (b = 0; b < (MLX_LONG1_DATA_SZ / 2); ++b)
TNU 0:dfe498e03679 875 {
TNU 0:dfe498e03679 876 //WTA: removed MLX_HDR_SZ
TNU 0:dfe498e03679 877 buf[a*(MLX_LONG1_DATA_SZ / 2) + b] = u16ptr[b];
TNU 0:dfe498e03679 878 }
TNU 0:dfe498e03679 879
TNU 0:dfe498e03679 880 }
TNU 0:dfe498e03679 881 trigger.write(0);
TNU 0:dfe498e03679 882 device.format(8,1);
TNU 0:dfe498e03679 883
TNU 0:dfe498e03679 884 END:
TNU 0:dfe498e03679 885 device.format(8,1);
TNU 0:dfe498e03679 886 return res;
TNU 0:dfe498e03679 887 }
TNU 0:dfe498e03679 888
TNU 0:dfe498e03679 889 int LidarSpi::GetTraceOne ( uint16_t *buf, uint16_t maxN, uint16_t nSam, uint16_t idx,int index , Serial* pc){
TNU 0:dfe498e03679 890 int res, a, b, i;
TNU 0:dfe498e03679 891 uint32_t cnt;
TNU 0:dfe498e03679 892 uint16_t rSz;
TNU 0:dfe498e03679 893 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 894 PACK_LONG1 rxL, txL;
TNU 0:dfe498e03679 895 uint8_t iserr;
TNU 0:dfe498e03679 896 uint16_t err;
TNU 0:dfe498e03679 897 uint32_t val = 0;
TNU 0:dfe498e03679 898
TNU 0:dfe498e03679 899 uint16_t * u16ptr;
TNU 0:dfe498e03679 900 //pc->printf("Buffer space required: %d\n\r", MAXTRCLEN);
TNU 0:dfe498e03679 901 // Required buffer space
TNU 0:dfe498e03679 902 if (maxN < MAXTRCLEN){
TNU 0:dfe498e03679 903 pc-printf("NOT ENOUGH BUFFER SPACEn\r");
TNU 0:dfe498e03679 904 return -1;}
TNU 0:dfe498e03679 905
TNU 0:dfe498e03679 906 // Divide by payload size to get number of packets
TNU 0:dfe498e03679 907 // const int nPack = MAXTRCLEN / MLX_LONG_DATA_SZ;
TNU 0:dfe498e03679 908 // WTA: change nPack to a variable, initially 64
TNU 0:dfe498e03679 909 int nPack = MAXTRCLEN / MLX_LONG1_DATA_SZ;
TNU 0:dfe498e03679 910 //pc->printf("npack: %d", nPack);
TNU 0:dfe498e03679 911 // Ensure transmitted packet is all zeros to not send trash
TNU 0:dfe498e03679 912 memset(&txL, 0, sizeof(PACK_LONG1));
TNU 0:dfe498e03679 913
TNU 0:dfe498e03679 914 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 915 // memset(rx, 0, sizeof(rx));
TNU 0:dfe498e03679 916
TNU 0:dfe498e03679 917
TNU 0:dfe498e03679 918 res = ReadReg(336, &val); // PORT_CHSEL
TNU 0:dfe498e03679 919 if (res < 0)
TNU 0:dfe498e03679 920 goto END;
TNU 0:dfe498e03679 921 val >>= 16;
TNU 0:dfe498e03679 922 //pc->printf("chsel = %d\n", val);
TNU 0:dfe498e03679 923 cnt = 0;
TNU 0:dfe498e03679 924 // Count how many channels are selected
TNU 0:dfe498e03679 925 for (i = 0; i < 16; i++) {
TNU 0:dfe498e03679 926 if (val & 0x1)
TNU 0:dfe498e03679 927 cnt++;
TNU 0:dfe498e03679 928 val >>= 1;
TNU 0:dfe498e03679 929 }
TNU 0:dfe498e03679 930 nPack *= cnt;
TNU 0:dfe498e03679 931 nPack /= 16;
TNU 0:dfe498e03679 932
TNU 0:dfe498e03679 933 res = ReadReg(332, &val); // PORT_OVR_ACCUM_ACQ_OVR
TNU 0:dfe498e03679 934 // pc->printf("PORT_OVR = %d\n", (val >> 16));
TNU 0:dfe498e03679 935 val = (val >> 16) & 3; // Get bits 0 and 1
TNU 0:dfe498e03679 936
TNU 0:dfe498e03679 937 if (res < 0){
TNU 0:dfe498e03679 938 pc->printf("ReadReg Error1\n\r");
TNU 0:dfe498e03679 939 goto END;}
TNU 0:dfe498e03679 940
TNU 0:dfe498e03679 941 if (val == 0){ //00 = 1
TNU 0:dfe498e03679 942 nPack /= 4;
TNU 0:dfe498e03679 943 }
TNU 0:dfe498e03679 944 else if (val == 1){ //01 = 2
TNU 0:dfe498e03679 945 nPack /= 4;
TNU 0:dfe498e03679 946 }
TNU 0:dfe498e03679 947 else if (val == 2){ //10 = 4
TNU 0:dfe498e03679 948 nPack /= 2;
TNU 0:dfe498e03679 949 }
TNU 0:dfe498e03679 950 else if (val == 3){ //11 = 8
TNU 0:dfe498e03679 951 nPack /= 1;
TNU 0:dfe498e03679 952 }
TNU 0:dfe498e03679 953 else {
TNU 0:dfe498e03679 954 pc->printf("GetTrace: bad value\n");
TNU 0:dfe498e03679 955 }
TNU 0:dfe498e03679 956
TNU 0:dfe498e03679 957
TNU 0:dfe498e03679 958 // Write 1 to PORT_ACQU register and then wait
TNU 0:dfe498e03679 959 res = ReadReg(0x146, &val); // PORT_ACQU
TNU 0:dfe498e03679 960 if (res < 0){
TNU 0:dfe498e03679 961 pc->printf("ReadReg Error2\n\r");
TNU 0:dfe498e03679 962 goto END;}
TNU 0:dfe498e03679 963 val = (val>>16) | 1;
TNU 0:dfe498e03679 964 res = WriteReg(0x146, val); // PORT_ACQU
TNU 0:dfe498e03679 965 if (res < 0){
TNU 0:dfe498e03679 966 pc->printf("WriteReg Error3\n\r");
TNU 0:dfe498e03679 967 goto END;}
TNU 0:dfe498e03679 968
TNU 0:dfe498e03679 969 // Wait till PORT_READY bit is set.
TNU 0:dfe498e03679 970 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 971 cnt = 0;
TNU 0:dfe498e03679 972 while (((val & 0x10000) >> 16 != 1) && (cnt < 500)) {
TNU 0:dfe498e03679 973 wait_us(50);
TNU 0:dfe498e03679 974 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 975 cnt++;
TNU 0:dfe498e03679 976 }
TNU 0:dfe498e03679 977
TNU 0:dfe498e03679 978 // Encode the request and send it
TNU 0:dfe498e03679 979 res = MLX_ReqReadTrc(tx);
TNU 0:dfe498e03679 980 if (res < 0)
TNU 0:dfe498e03679 981 goto END;
TNU 0:dfe498e03679 982
TNU 0:dfe498e03679 983
TNU 0:dfe498e03679 984 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 985 //----SHORT PACKET EXCHANGE--------
TNU 0:dfe498e03679 986 res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), 0);
TNU 0:dfe498e03679 987 if (res < 0)
TNU 0:dfe498e03679 988 goto END;
TNU 0:dfe498e03679 989
TNU 0:dfe498e03679 990 pc->printf("MOSI ReqReadTrace:\t 0x");
TNU 0:dfe498e03679 991 for(int i=0;i<(sizeof(tx));i++){
TNU 0:dfe498e03679 992 uint8_t* pnt=(uint8_t*)tx;
TNU 0:dfe498e03679 993 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 994 }
TNU 0:dfe498e03679 995 pc->printf("\n");
TNU 0:dfe498e03679 996 pc->printf("MISO ReqReadTrace:\t 0x");
TNU 0:dfe498e03679 997 for(int i=0;i<(sizeof(tx));i++){
TNU 0:dfe498e03679 998 uint8_t* pnt=(uint8_t*)rx;
TNU 0:dfe498e03679 999 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 1000 }
TNU 0:dfe498e03679 1001 pc->printf("\n");
TNU 0:dfe498e03679 1002 pc->printf("-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n");
TNU 0:dfe498e03679 1003 //----PACKET EXCHANGE
TNU 0:dfe498e03679 1004 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 1005
TNU 0:dfe498e03679 1006
TNU 0:dfe498e03679 1007 // Optional status decoding
TNU 0:dfe498e03679 1008 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 0:dfe498e03679 1009 if (res < 0 || iserr)
TNU 0:dfe498e03679 1010 goto END;
TNU 0:dfe498e03679 1011
TNU 0:dfe498e03679 1012
TNU 0:dfe498e03679 1013 // Temporary pointer to uint16 data, for simplicity purpose
TNU 0:dfe498e03679 1014 u16ptr = (uint16_t*)rxL.data;
TNU 0:dfe498e03679 1015 //device.format(16,1);
TNU 0:dfe498e03679 1016 trigger.write(1);
TNU 0:dfe498e03679 1017 for (a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 1018 {
TNU 0:dfe498e03679 1019 res = MLX_EncodeStatusL1(&txL, 0, 0);
TNU 0:dfe498e03679 1020 if (res < 0){
TNU 0:dfe498e03679 1021 res = -7;
TNU 0:dfe498e03679 1022 goto END;
TNU 0:dfe498e03679 1023 }
TNU 0:dfe498e03679 1024
TNU 0:dfe498e03679 1025 //Tools::Wait(10);
TNU 0:dfe498e03679 1026 wait_us(10);
TNU 0:dfe498e03679 1027
TNU 0:dfe498e03679 1028 // Clock the remaining long packets
TNU 0:dfe498e03679 1029 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG1));
TNU 0:dfe498e03679 1030 // pc->printf("\nRXL%d = 0x%02x%02x%02x...[x294 bytes]...%02x%02x%02x\n", a + 1, rxL.buf[0], rxL.buf[1], rxL.buf[2], rxL.buf[297], rxL.buf[298], rxL.buf[299]);
TNU 0:dfe498e03679 1031 // pc->printf("\nRXL%d = 0x%02x%02x%02x%02x%02x%02x\n", a + 1, rxL.buf[2], rxL.buf[3], rxL.buf[4], rxL.buf[5], rxL.buf[6], rxL.buf[7]);
TNU 0:dfe498e03679 1032 if (res < 0){
TNU 0:dfe498e03679 1033 res = -8;
TNU 0:dfe498e03679 1034 goto END;
TNU 0:dfe498e03679 1035 }
TNU 0:dfe498e03679 1036
TNU 0:dfe498e03679 1037
TNU 0:dfe498e03679 1038 // Decode the long responses, then extract data values
TNU 0:dfe498e03679 1039 res = MLX_DecodeResL1(&rxL);
TNU 0:dfe498e03679 1040 if ((res < 0)){
TNU 0:dfe498e03679 1041 pc->printf("LONG READ ERROR: Stopped at the %d long message\n", a);
TNU 0:dfe498e03679 1042 pc->printf("TXLONG MOSI Response: ");
TNU 0:dfe498e03679 1043 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 1044 uint8_t* pnt=(uint8_t*)(&txL);
TNU 0:dfe498e03679 1045 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 1046 }
TNU 0:dfe498e03679 1047 pc->printf("\n\r");
TNU 0:dfe498e03679 1048 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 1049 pc->printf("TXLONG MISO Response: ");
TNU 0:dfe498e03679 1050 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 1051 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 0:dfe498e03679 1052 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 1053
TNU 0:dfe498e03679 1054 }
TNU 0:dfe498e03679 1055 pc->printf("\n\r");
TNU 0:dfe498e03679 1056 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 1057 //printf("last RXL = \n");
TNU 0:dfe498e03679 1058 //for (i = 0; i < 300; i++) {
TNU 0:dfe498e03679 1059 // pc->printf("%02x", rxL.buf[i]);
TNU 0:dfe498e03679 1060 // if (((i + 1) % 8) == 0)
TNU 0:dfe498e03679 1061 // pc->printf("\n");
TNU 0:dfe498e03679 1062 //}
TNU 0:dfe498e03679 1063 //printf("\n");
TNU 0:dfe498e03679 1064 res = -9;
TNU 0:dfe498e03679 1065 goto END;
TNU 0:dfe498e03679 1066 }
TNU 0:dfe498e03679 1067
TNU 0:dfe498e03679 1068 // Copy the returned data into the user buffer
TNU 0:dfe498e03679 1069 // WTA: only half the MLX_LONG_DATA_SZ because we copy 2 bytes at a time
TNU 0:dfe498e03679 1070 for (b = 0; b < (MLX_LONG1_DATA_SZ / 2); ++b)
TNU 0:dfe498e03679 1071 {
TNU 0:dfe498e03679 1072 //WTA: removed MLX_HDR_SZ
TNU 0:dfe498e03679 1073 buf[a*(MLX_LONG1_DATA_SZ / 2) + b] = u16ptr[b];
TNU 0:dfe498e03679 1074 }
TNU 0:dfe498e03679 1075 if(a<64){
TNU 0:dfe498e03679 1076 pc->printf("Trace packet %d MOSI: \n\t\t\t\t\t0x", a);
TNU 0:dfe498e03679 1077 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 1078 uint8_t* pnt=(uint8_t*)(&txL);
TNU 0:dfe498e03679 1079 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 1080 if(((i %30) ==0)&&(i>0))pc->printf("\n\t\t\t\t\t");
TNU 0:dfe498e03679 1081 }
TNU 0:dfe498e03679 1082 pc->printf("\n\r");
TNU 0:dfe498e03679 1083 pc->printf("Trace packet %d MISO: \n\t\t\t\t\t0x", a);
TNU 0:dfe498e03679 1084 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 1085 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 0:dfe498e03679 1086 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 1087 if(((i %30) ==0)&&(i>0))pc->printf("\n\t\t\t\t\t");
TNU 0:dfe498e03679 1088 }
TNU 0:dfe498e03679 1089 pc->printf("\n\r");
TNU 0:dfe498e03679 1090 pc->printf("-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 1091 }
TNU 0:dfe498e03679 1092
TNU 0:dfe498e03679 1093 }
TNU 0:dfe498e03679 1094 trigger.write(0);
TNU 0:dfe498e03679 1095 device.format(8,1);
TNU 0:dfe498e03679 1096
TNU 0:dfe498e03679 1097 END:
TNU 0:dfe498e03679 1098 device.format(8,1);
TNU 0:dfe498e03679 1099 return res;
TNU 0:dfe498e03679 1100 }
TNU 0:dfe498e03679 1101
TNU 0:dfe498e03679 1102
TNU 6:748062f3de21 1103 int LidarSpi::LoadPatch (const char *patch, Serial * pc){
TNU 6:748062f3de21 1104 int res=0;
TNU 0:dfe498e03679 1105 uint16_t rSz;
TNU 0:dfe498e03679 1106 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 1107 PACK_LONG2 rxL, txL;
TNU 0:dfe498e03679 1108 uint8_t iserr;
TNU 0:dfe498e03679 1109 uint16_t err;
TNU 0:dfe498e03679 1110 //uint32_t val = 0;
TNU 0:dfe498e03679 1111 uint16_t nWordsRemaining;
TNU 0:dfe498e03679 1112 uint16_t nPack;
TNU 6:748062f3de21 1113 uint16_t nWords;
TNU 6:748062f3de21 1114
TNU 6:748062f3de21 1115 uint8_t memory[512];
TNU 6:748062f3de21 1116 uint16_t addrStart, startLine, nBytes ;
TNU 6:748062f3de21 1117 startLine=0;
TNU 6:748062f3de21 1118 memset(memory, 0, 512);
TNU 6:748062f3de21 1119
TNU 0:dfe498e03679 1120
TNU 6:748062f3de21 1121 res=WriteReg(0x1d8, 128);
TNU 6:748062f3de21 1122 if (res<0){
TNU 6:748062f3de21 1123 pc->printf("PORT_LONG_PACKET_SIZE Write error\n\r");
TNU 0:dfe498e03679 1124 goto END;
TNU 6:748062f3de21 1125 }
TNU 6:748062f3de21 1126
TNU 6:748062f3de21 1127 wait_us(5);
TNU 6:748062f3de21 1128 pc->printf("Start loading fragments \n\r");
TNU 6:748062f3de21 1129 int count=0;
TNU 6:748062f3de21 1130 while(LoadPatchFragment(patch,&addrStart, &startLine, &nBytes, memory, pc)==-5){
TNU 6:748062f3de21 1131 Trigger(0);
TNU 6:748062f3de21 1132 pc->printf("Load fragment %d\n\r", count);
TNU 6:748062f3de21 1133 for(int i =0; i<nBytes;i++){
TNU 6:748062f3de21 1134 //pc->printf("Addr[%d] : %d\n\r", addrStart+i, memory[i] );
TNU 6:748062f3de21 1135 }
TNU 6:748062f3de21 1136 pc->printf("addrStart: %d, startLine %d, nBytes: %d\n\r",addrStart, startLine, nBytes);
TNU 6:748062f3de21 1137 nWords= nBytes / 2;
TNU 6:748062f3de21 1138
TNU 6:748062f3de21 1139 // Ensure all packets are all zeros to not send trash
TNU 6:748062f3de21 1140 memset(tx, 0, sizeof(tx));
TNU 6:748062f3de21 1141 memset(&rxL, 0, sizeof(rxL));
TNU 6:748062f3de21 1142
TNU 6:748062f3de21 1143 //The Number of words to be transferred can be a multiple of LONG_DATA_SZ. Incase it is not
TNU 6:748062f3de21 1144 //multiple of LONG_DATA_SZ, we do last packet with partial data and remaining words as 0
TNU 6:748062f3de21 1145 nPack = nWords / (MLX_LONG2_DATA_SZ / 2) + (nWords % (MLX_LONG2_DATA_SZ / 2) == 0 ? 0 :1);
TNU 6:748062f3de21 1146 pc->printf("npack: %d\n\r", nPack);
TNU 6:748062f3de21 1147 // Encode the request and send it
TNU 6:748062f3de21 1148 res = MLX_ReqWriteFW(tx, nPack, addrStart);
TNU 6:748062f3de21 1149 if (res < 0){
TNU 6:748062f3de21 1150 pc->printf("Err @ 1");
TNU 6:748062f3de21 1151 goto END; }
TNU 6:748062f3de21 1152
TNU 6:748062f3de21 1153 wait_us(16);
TNU 6:748062f3de21 1154 res = TxPacket((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx));
TNU 6:748062f3de21 1155 if (res < 0){
TNU 6:748062f3de21 1156 pc->printf("Err @ 2");
TNU 6:748062f3de21 1157 goto END; }
TNU 6:748062f3de21 1158 //Trigger(1);
TNU 6:748062f3de21 1159 /*
TNU 6:748062f3de21 1160 pc->printf("ReqWriteFirmware MOSI: \n\r0x");
TNU 6:748062f3de21 1161 for(int k=0;k<(sizeof(PACK_LONG2));k++){
TNU 6:748062f3de21 1162 uint8_t* pnt=(uint8_t*)(&txL);
TNU 6:748062f3de21 1163 pc->printf("%02X", *(pnt+k));
TNU 6:748062f3de21 1164 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 6:748062f3de21 1165 }
TNU 6:748062f3de21 1166 pc->printf("\n\r");
TNU 6:748062f3de21 1167 pc->printf("ReqWriteFirmware MISO: \n\r0x");
TNU 6:748062f3de21 1168 for(int k=0;k<(sizeof(PACK_LONG2));k++){
TNU 6:748062f3de21 1169 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 6:748062f3de21 1170 pc->printf("%02X", *(pnt+k));
TNU 6:748062f3de21 1171 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 6:748062f3de21 1172 }
TNU 6:748062f3de21 1173 pc->printf("\n\r-------------------\n\r");
TNU 6:748062f3de21 1174 */
TNU 6:748062f3de21 1175
TNU 6:748062f3de21 1176
TNU 6:748062f3de21 1177 wait_us(5);
TNU 6:748062f3de21 1178
TNU 6:748062f3de21 1179 // Optional status decoding
TNU 6:748062f3de21 1180 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 6:748062f3de21 1181 if (res < 0 || iserr){
TNU 6:748062f3de21 1182 pc->printf("Err @ 3");
TNU 6:748062f3de21 1183
TNU 6:748062f3de21 1184 pc->printf("LOAD PATCH REQ response MISO: \n\r0x");
TNU 6:748062f3de21 1185 for(int k=0;k<(sizeof(PACK_LONG2));k++){
TNU 6:748062f3de21 1186 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 6:748062f3de21 1187 pc->printf("%02X", *(pnt+k));
TNU 6:748062f3de21 1188 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 6:748062f3de21 1189 }
TNU 6:748062f3de21 1190 pc->printf("\n\r");
TNU 6:748062f3de21 1191
TNU 6:748062f3de21 1192
TNU 6:748062f3de21 1193 goto END; }
TNU 6:748062f3de21 1194
TNU 6:748062f3de21 1195 nWordsRemaining = nWords;
TNU 6:748062f3de21 1196 for (uint a = 0; a < nPack; ++a)
TNU 6:748062f3de21 1197 {
TNU 6:748062f3de21 1198 uint16_t size;
TNU 6:748062f3de21 1199 if (nWordsRemaining > (MLX_LONG2_DATA_SZ / 2))
TNU 6:748062f3de21 1200 size = MLX_LONG2_DATA_SZ / 2;
TNU 6:748062f3de21 1201 else
TNU 6:748062f3de21 1202 size = nWordsRemaining;
TNU 0:dfe498e03679 1203
TNU 6:748062f3de21 1204 res = MLX_WriteDataL2(&txL, size, a, &memory[a*MLX_LONG2_DATA_SZ]);
TNU 6:748062f3de21 1205 if (res < 0){
TNU 6:748062f3de21 1206 res = -7;
TNU 6:748062f3de21 1207 goto END;
TNU 6:748062f3de21 1208 }
TNU 6:748062f3de21 1209
TNU 6:748062f3de21 1210 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
TNU 6:748062f3de21 1211
TNU 6:748062f3de21 1212 //Trigger(0);
TNU 6:748062f3de21 1213 wait_us(8);
TNU 0:dfe498e03679 1214
TNU 6:748062f3de21 1215 /*
TNU 6:748062f3de21 1216 pc->printf("LOAD PATCH LONG RESPONSE %d of fragment %d MOSI: \n\r0x", a,count);
TNU 6:748062f3de21 1217 for(int k=0;k<(sizeof(PACK_LONG2));k++){
TNU 6:748062f3de21 1218 uint8_t* pnt=(uint8_t*)(&txL);
TNU 6:748062f3de21 1219 pc->printf("%02X", *(pnt+k));
TNU 6:748062f3de21 1220 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 6:748062f3de21 1221 }
TNU 6:748062f3de21 1222 pc->printf("\n\r");
TNU 6:748062f3de21 1223 pc->printf("LOAD PATCH LONG RESPONSE %d of fragment %d MISO: \n\r0x", a,count);
TNU 6:748062f3de21 1224 for(int k=0;k<(sizeof(PACK_LONG2));k++){
TNU 6:748062f3de21 1225 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 6:748062f3de21 1226 pc->printf("%02X", *(pnt+k));
TNU 6:748062f3de21 1227 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 6:748062f3de21 1228 }
TNU 6:748062f3de21 1229 pc->printf("\n\r-----------\n\r");
TNU 6:748062f3de21 1230 */
TNU 6:748062f3de21 1231
TNU 6:748062f3de21 1232
TNU 6:748062f3de21 1233 res = MLX_DecodeResL2(&rxL);
TNU 6:748062f3de21 1234 if (res < 0){
TNU 6:748062f3de21 1235 pc->printf("LONG WRITE ERROR: Stopped at the %d long message, res=%d\n", a, res);
TNU 6:748062f3de21 1236
TNU 6:748062f3de21 1237 pc->printf("LOAD PATCH LONG RESPONSE %d MOSI: \n\r0x", a);
TNU 6:748062f3de21 1238 for(int k=0;k<(sizeof(PACK_LONG2));k++){
TNU 6:748062f3de21 1239 uint8_t* pnt=(uint8_t*)(&txL);
TNU 6:748062f3de21 1240 pc->printf("%02X", *(pnt+k));
TNU 6:748062f3de21 1241 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 6:748062f3de21 1242 }
TNU 6:748062f3de21 1243 pc->printf("\n\r");
TNU 6:748062f3de21 1244 pc->printf("LOAD PATCH LONG RESPONSE %d MISO: \n\r0x", a);
TNU 6:748062f3de21 1245 for(int k=0;k<(sizeof(PACK_LONG2));k++){
TNU 6:748062f3de21 1246 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 6:748062f3de21 1247 pc->printf("%02X", *(pnt+k));
TNU 6:748062f3de21 1248 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 6:748062f3de21 1249 }
TNU 6:748062f3de21 1250 pc->printf("\n\r");
TNU 6:748062f3de21 1251 res = -9;
TNU 6:748062f3de21 1252 goto END;
TNU 6:748062f3de21 1253 }
TNU 6:748062f3de21 1254
TNU 6:748062f3de21 1255 nWordsRemaining = nWords - size;
TNU 6:748062f3de21 1256
TNU 6:748062f3de21 1257 }
TNU 6:748062f3de21 1258 count++;
TNU 6:748062f3de21 1259 //LAST STATUS LONG PACKET FROM 75320 to get status of last write long
TNU 6:748062f3de21 1260 res = MLX_EncodeStatusL2(&txL, 0, 0);
TNU 6:748062f3de21 1261 if (res < 0) {
TNU 0:dfe498e03679 1262 res = -7;
TNU 0:dfe498e03679 1263 goto END;
TNU 0:dfe498e03679 1264 }
TNU 6:748062f3de21 1265
TNU 6:748062f3de21 1266 // Clock the remaining long packets
TNU 0:dfe498e03679 1267 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
TNU 6:748062f3de21 1268 wait_us(11);
TNU 0:dfe498e03679 1269 if (res < 0){
TNU 6:748062f3de21 1270 pc->printf("Err @ 4");
TNU 6:748062f3de21 1271 goto END; }
TNU 6:748062f3de21 1272
TNU 6:748062f3de21 1273
TNU 6:748062f3de21 1274
TNU 6:748062f3de21 1275
TNU 6:748062f3de21 1276 memset(memory, 0, 512);
TNU 6:748062f3de21 1277 wait_us(12);
TNU 6:748062f3de21 1278 }
TNU 6:748062f3de21 1279 /*
TNU 6:748062f3de21 1280 pc->printf("Status long packet to check status after last fragment : %d, res=%d\n", count, res);
TNU 6:748062f3de21 1281
TNU 6:748062f3de21 1282 pc->printf("LOAD PATCH LONG STATUS %d MOSI: \n\r0x", count);
TNU 6:748062f3de21 1283 for(int k=0;k<(sizeof(PACK_LONG2));k++){
TNU 6:748062f3de21 1284 uint8_t* pnt=(uint8_t*)(&txL);
TNU 6:748062f3de21 1285 pc->printf("%02X", *(pnt+k));
TNU 6:748062f3de21 1286 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 0:dfe498e03679 1287 }
TNU 6:748062f3de21 1288 pc->printf("\n\r");
TNU 6:748062f3de21 1289 pc->printf("LOAD PATCH LONG RESPONSE %d MISO: \n\r0x", count);
TNU 6:748062f3de21 1290 for(int k=0;k<(sizeof(PACK_LONG2));k++){
TNU 6:748062f3de21 1291 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 6:748062f3de21 1292 pc->printf("%02X", *(pnt+k));
TNU 6:748062f3de21 1293 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 0:dfe498e03679 1294 }
TNU 6:748062f3de21 1295 pc->printf("\n\r-----------\n\r");
TNU 6:748062f3de21 1296 */
TNU 0:dfe498e03679 1297 // Change jump table pointer.
TNU 6:748062f3de21 1298 //res=LoadPatchFragment(patch,&addrStart, &startLine, &nBytes, memory, pc);
TNU 6:748062f3de21 1299 //pc->printf("Return from a loadPatchFragment: res = %d \n\r", res);
TNU 6:748062f3de21 1300 wait_ms(100);
TNU 6:748062f3de21 1301
TNU 6:748062f3de21 1302 if (count>3){
TNU 6:748062f3de21 1303 pc->printf("Change jumptable... \n\r");
TNU 6:748062f3de21 1304 res = WriteReg(0x1000, 0x7000, pc); // write addr: 0x1000 value:0x7000
TNU 6:748062f3de21 1305 if (res < 0){
TNU 6:748062f3de21 1306 pc->printf("Err @ 5");
TNU 6:748062f3de21 1307 goto END;
TNU 6:748062f3de21 1308 }
TNU 6:748062f3de21 1309 }
TNU 6:748062f3de21 1310 else pc->printf("Failed to read the file\n\r");
TNU 0:dfe498e03679 1311
TNU 0:dfe498e03679 1312 END:
TNU 0:dfe498e03679 1313 return res;
TNU 0:dfe498e03679 1314 }
TNU 0:dfe498e03679 1315
TNU 0:dfe498e03679 1316 void LidarSpi::Trigger(int level){
TNU 0:dfe498e03679 1317 trigger.write(level);
TNU 0:dfe498e03679 1318 }
TNU 6:748062f3de21 1319
TNU 6:748062f3de21 1320
TNU 6:748062f3de21 1321 int LidarSpi::LoadPatchFragment(const char *patch, uint16_t *addrStart, uint16_t *startLine, uint16_t *nBytes, uint8_t *memory, Serial* pc){
TNU 6:748062f3de21 1322 char line[100];
TNU 6:748062f3de21 1323 FILE *fin;
TNU 6:748062f3de21 1324 uint16_t addr, n, status;
TNU 6:748062f3de21 1325 uint8_t bytes[256];
TNU 6:748062f3de21 1326 uint16_t i, total = 0, lineno = 1;
TNU 6:748062f3de21 1327 uint16_t minaddr = 65535, maxaddr = 0;
TNU 6:748062f3de21 1328
TNU 6:748062f3de21 1329 uint16_t prevAddr = 0;
TNU 6:748062f3de21 1330 uint16_t lineCount = *startLine;
TNU 6:748062f3de21 1331 uint16_t lines = 0;
TNU 6:748062f3de21 1332
TNU 6:748062f3de21 1333 bool isFirst = true;
TNU 6:748062f3de21 1334
TNU 6:748062f3de21 1335 *nBytes = 0;
TNU 6:748062f3de21 1336 if (strlen(patch) == 0) {
TNU 6:748062f3de21 1337 //printf(" Can't load a file without the filename.");
TNU 6:748062f3de21 1338 return -9;
TNU 6:748062f3de21 1339 }
TNU 6:748062f3de21 1340 fin = fopen(patch, "r");
TNU 6:748062f3de21 1341 if (fin == NULL) {
TNU 6:748062f3de21 1342 pc->printf(" Can't open file '%s' for reading.\n\r", patch);
TNU 6:748062f3de21 1343 return -10;
TNU 6:748062f3de21 1344 }
TNU 6:748062f3de21 1345 // printf("Patch file %s opened\n", filename);
TNU 6:748062f3de21 1346
TNU 6:748062f3de21 1347 while (true) {
TNU 6:748062f3de21 1348 line[0] = '\0';
TNU 6:748062f3de21 1349 do {
TNU 6:748062f3de21 1350 fgets(line, 1000, fin);
TNU 6:748062f3de21 1351 lines++;
TNU 6:748062f3de21 1352 } while (lines < (*startLine));
TNU 6:748062f3de21 1353 //pc->printf("Startline: %d, lines: %d\n\r", *startLine,lines);
TNU 6:748062f3de21 1354
TNU 6:748062f3de21 1355 if (line[strlen(line) - 1] == '\n') line[strlen(line) - 1] = '\0';
TNU 6:748062f3de21 1356 if (line[strlen(line) - 1] == '\r') line[strlen(line) - 1] = '\0';
TNU 6:748062f3de21 1357 *addrStart= 0;
TNU 6:748062f3de21 1358 if (parse_hex_line(line, bytes, &addr, &n, &status)) {
TNU 6:748062f3de21 1359 //cout << "Parse OK\n\r";
TNU 6:748062f3de21 1360 //pc->printf("Line: %s\n\r", line);
TNU 6:748062f3de21 1361 if (!(isFirst | ((addr == prevAddr + 1) && (((*nBytes)+n) < 512)))) {
TNU 6:748062f3de21 1362 *addrStart = minaddr;
TNU 6:748062f3de21 1363 *startLine = lines;
TNU 6:748062f3de21 1364 fclose(fin);
TNU 6:748062f3de21 1365 return -5;
TNU 6:748062f3de21 1366 }
TNU 6:748062f3de21 1367
TNU 6:748062f3de21 1368 if (status == 0) { /* data */
TNU 6:748062f3de21 1369 //cout << "Status=0";
TNU 6:748062f3de21 1370 isFirst = false;
TNU 6:748062f3de21 1371 for (i = 0; i <= (n - 1); i++) {
TNU 6:748062f3de21 1372 if (i % 2 == 0)
TNU 6:748062f3de21 1373 memory[*nBytes+i] = bytes[i + 1] & 255; //Assumption even no of bytes per line
TNU 6:748062f3de21 1374 else
TNU 6:748062f3de21 1375 memory[*nBytes+i] = bytes[i - 1] & 255;
TNU 6:748062f3de21 1376 total++;
TNU 6:748062f3de21 1377 if (addr < minaddr) minaddr = addr;
TNU 6:748062f3de21 1378 if (addr > maxaddr) maxaddr = addr;
TNU 6:748062f3de21 1379 prevAddr = addr;
TNU 6:748062f3de21 1380 addr++;
TNU 6:748062f3de21 1381 }
TNU 6:748062f3de21 1382 }
TNU 6:748062f3de21 1383 if (status == 1) { /* end of file */
TNU 6:748062f3de21 1384 fclose(fin);
TNU 6:748062f3de21 1385 //printf("load_file parsed %d bytes between:", total);
TNU 6:748062f3de21 1386 //printf(" %04X to %04X\n", minaddr, maxaddr);
TNU 6:748062f3de21 1387 *addrStart= minaddr;
TNU 6:748062f3de21 1388 pc->printf("End of file\n\r");
TNU 6:748062f3de21 1389 return total;
TNU 6:748062f3de21 1390 }
TNU 6:748062f3de21 1391 if (status == 2) /* begin of file */
TNU 6:748062f3de21 1392 goto NEXT;
TNU 6:748062f3de21 1393 *nBytes += n;
TNU 6:748062f3de21 1394 }
TNU 6:748062f3de21 1395 else {
TNU 6:748062f3de21 1396 printf(" Error: '%s', line: %d\n", patch, lineno);
TNU 6:748062f3de21 1397 }
TNU 6:748062f3de21 1398 lineCount++;
TNU 6:748062f3de21 1399 NEXT: lineno++;
TNU 6:748062f3de21 1400 }
TNU 6:748062f3de21 1401 fclose(fin);
TNU 6:748062f3de21 1402 return 0;
TNU 6:748062f3de21 1403
TNU 6:748062f3de21 1404 }
TNU 6:748062f3de21 1405 int LidarSpi::parse_hex_line(char *theline, uint8_t bytes[], uint16_t *addr, uint16_t *num, uint16_t *code){
TNU 6:748062f3de21 1406 unsigned int sum, len, cksum;
TNU 6:748062f3de21 1407 unsigned int newAddr, newCode, newByte;
TNU 6:748062f3de21 1408
TNU 6:748062f3de21 1409
TNU 6:748062f3de21 1410 char *ptr;
TNU 6:748062f3de21 1411 //cout << "Start parsing\n\r";
TNU 6:748062f3de21 1412 *num = 0;
TNU 6:748062f3de21 1413 if (theline[0] != ':') return 0;
TNU 6:748062f3de21 1414 if (strlen(theline) < 11) return 0;
TNU 6:748062f3de21 1415 ptr = theline + 1;
TNU 6:748062f3de21 1416 if (!sscanf(ptr, "%02x", &len)) return 0;
TNU 6:748062f3de21 1417 ptr += 2;
TNU 6:748062f3de21 1418 if (strlen(theline) < (11 + (len * 2))) return 0;
TNU 6:748062f3de21 1419 if (!sscanf(ptr, "%04x", &newAddr)) return 0;
TNU 6:748062f3de21 1420 *addr=(uint16_t)newAddr;
TNU 6:748062f3de21 1421 ptr += 4;
TNU 6:748062f3de21 1422 //printf("Line: length=%d Addr=%d\n", len, *addr);
TNU 6:748062f3de21 1423 if (!sscanf(ptr, "%02x", &newCode)) return 0;
TNU 6:748062f3de21 1424 *code=(uint16_t)newCode;
TNU 6:748062f3de21 1425 ptr += 2;
TNU 6:748062f3de21 1426 sum = (len & 255) + ((*addr >> 8) & 255) + (*addr & 255) + (*code & 255);
TNU 6:748062f3de21 1427 while (*num != len) {
TNU 6:748062f3de21 1428 if (!sscanf(ptr, "%02x", &newByte )) return 0;
TNU 6:748062f3de21 1429 bytes[*num]=(uint8_t)newByte;
TNU 6:748062f3de21 1430 ptr += 2;
TNU 6:748062f3de21 1431 sum += bytes[*num] & 255;
TNU 6:748062f3de21 1432 (*num)++;
TNU 6:748062f3de21 1433 if (*num >= 256) return 0;
TNU 6:748062f3de21 1434 }
TNU 6:748062f3de21 1435 if (!sscanf(ptr, "%02x", &cksum)) return 0;
TNU 6:748062f3de21 1436 if (((sum & 255) + (cksum & 255)) & 255) return 0; /* checksum error */
TNU 6:748062f3de21 1437 return 1;
TNU 6:748062f3de21 1438 }