Tom N / LidarSpi

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

Dependents:   MLX75320_API

LidarSpi.cpp@7:c47612b25c77, 2016-03-29 (annotated)

Committer:
TNU
Date:
Tue Mar 29 13:34:25 2016 +0000
Revision:
7:c47612b25c77
Parent:
6:748062f3de21
Child:
8:d49102c10940
LoadPatch works;

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 7:c47612b25c77 727 for (int 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 7:c47612b25c77 773
TNU 0:dfe498e03679 774 // Wait till PORT_READY bit is set.
TNU 0:dfe498e03679 775 cnt = 0;
TNU 5:87e211a23654 776 while((!dataReady.read())&& (cnt<2000)){
TNU 3:9ed1d493c235 777 wait_us(50);
TNU 3:9ed1d493c235 778 cnt++;
TNU 0:dfe498e03679 779 }
TNU 3:9ed1d493c235 780
TNU 5:87e211a23654 781 res = ReadReg(470, &val); // PORT_READY
TNU 5:87e211a23654 782 val=val>>16;
TNU 5:87e211a23654 783 pc->printf("PORT READY: %x\n\r",val);
TNU 5:87e211a23654 784
TNU 3:9ed1d493c235 785 trigger.write(0);
TNU 3:9ed1d493c235 786 pc-printf("Count: %d\n\r", cnt);
TNU 0:dfe498e03679 787
TNU 0:dfe498e03679 788 // Encode the request and send it
TNU 0:dfe498e03679 789 res = MLX_ReqReadTrc(tx);
TNU 0:dfe498e03679 790 if (res < 0)
TNU 0:dfe498e03679 791 goto END;
TNU 0:dfe498e03679 792
TNU 0:dfe498e03679 793
TNU 0:dfe498e03679 794 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 795 //----SHORT PACKET EXCHANGE--------
TNU 0:dfe498e03679 796 res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), 0);
TNU 0:dfe498e03679 797 if (res < 0)
TNU 0:dfe498e03679 798 goto END;
TNU 0:dfe498e03679 799 //----PACKET EXCHANGE
TNU 0:dfe498e03679 800 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 801
TNU 0:dfe498e03679 802
TNU 0:dfe498e03679 803 // Optional status decoding
TNU 0:dfe498e03679 804 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 0:dfe498e03679 805 if (res < 0 || iserr)
TNU 0:dfe498e03679 806 goto END;
TNU 0:dfe498e03679 807
TNU 0:dfe498e03679 808
TNU 0:dfe498e03679 809 // Temporary pointer to uint16 data, for simplicity purpose
TNU 0:dfe498e03679 810 u16ptr = (uint16_t*)rxL.data;
TNU 0:dfe498e03679 811 //device.format(16,1);
TNU 3:9ed1d493c235 812 //trigger.write(1);
TNU 0:dfe498e03679 813 for (a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 814 {
TNU 0:dfe498e03679 815 res = MLX_EncodeStatusL1(&txL, 0, 0);
TNU 0:dfe498e03679 816 if (res < 0){
TNU 0:dfe498e03679 817 res = -7;
TNU 0:dfe498e03679 818 goto END;
TNU 0:dfe498e03679 819 }
TNU 0:dfe498e03679 820
TNU 0:dfe498e03679 821 //Tools::Wait(10);
TNU 0:dfe498e03679 822 wait_us(10);
TNU 0:dfe498e03679 823
TNU 0:dfe498e03679 824 // Clock the remaining long packets
TNU 0:dfe498e03679 825 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG1));
TNU 0:dfe498e03679 826 // 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 827 // 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 828 if (res < 0){
TNU 0:dfe498e03679 829 res = -8;
TNU 0:dfe498e03679 830 goto END;
TNU 0:dfe498e03679 831 }
TNU 0:dfe498e03679 832
TNU 0:dfe498e03679 833 // Decode the long responses, then extract data values
TNU 0:dfe498e03679 834 res = MLX_DecodeResL1(&rxL);
TNU 0:dfe498e03679 835 if ((res < 0)){
TNU 0:dfe498e03679 836 pc->printf("LONG READ ERROR: Stopped at the %d long message\n", a);
TNU 0:dfe498e03679 837 pc->printf("TXLONG MOSI Response: ");
TNU 0:dfe498e03679 838 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 839 uint8_t* pnt=(uint8_t*)(&txL);
TNU 0:dfe498e03679 840 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 841 }
TNU 0:dfe498e03679 842 pc->printf("\n\r");
TNU 0:dfe498e03679 843 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 844 pc->printf("TXLONG MISO Response: ");
TNU 0:dfe498e03679 845 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 846 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 0:dfe498e03679 847 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 848
TNU 0:dfe498e03679 849 }
TNU 0:dfe498e03679 850 pc->printf("\n\r");
TNU 0:dfe498e03679 851 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 852 //printf("last RXL = \n");
TNU 0:dfe498e03679 853 //for (i = 0; i < 300; i++) {
TNU 0:dfe498e03679 854 // pc->printf("%02x", rxL.buf[i]);
TNU 0:dfe498e03679 855 // if (((i + 1) % 8) == 0)
TNU 0:dfe498e03679 856 // pc->printf("\n");
TNU 0:dfe498e03679 857 //}
TNU 0:dfe498e03679 858 //printf("\n");
TNU 0:dfe498e03679 859 res = -9;
TNU 0:dfe498e03679 860 goto END;
TNU 0:dfe498e03679 861 }
TNU 0:dfe498e03679 862
TNU 0:dfe498e03679 863 // Copy the returned data into the user buffer
TNU 0:dfe498e03679 864 // WTA: only half the MLX_LONG_DATA_SZ because we copy 2 bytes at a time
TNU 0:dfe498e03679 865 for (b = 0; b < (MLX_LONG1_DATA_SZ / 2); ++b)
TNU 0:dfe498e03679 866 {
TNU 0:dfe498e03679 867 //WTA: removed MLX_HDR_SZ
TNU 0:dfe498e03679 868 buf[a*(MLX_LONG1_DATA_SZ / 2) + b] = u16ptr[b];
TNU 0:dfe498e03679 869 }
TNU 0:dfe498e03679 870
TNU 0:dfe498e03679 871 }
TNU 0:dfe498e03679 872 trigger.write(0);
TNU 0:dfe498e03679 873 device.format(8,1);
TNU 0:dfe498e03679 874
TNU 0:dfe498e03679 875 END:
TNU 0:dfe498e03679 876 device.format(8,1);
TNU 0:dfe498e03679 877 return res;
TNU 0:dfe498e03679 878 }
TNU 0:dfe498e03679 879
TNU 0:dfe498e03679 880 int LidarSpi::GetTraceOne ( uint16_t *buf, uint16_t maxN, uint16_t nSam, uint16_t idx,int index , Serial* pc){
TNU 0:dfe498e03679 881 int res, a, b, i;
TNU 0:dfe498e03679 882 uint32_t cnt;
TNU 0:dfe498e03679 883 uint16_t rSz;
TNU 0:dfe498e03679 884 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 885 PACK_LONG1 rxL, txL;
TNU 0:dfe498e03679 886 uint8_t iserr;
TNU 0:dfe498e03679 887 uint16_t err;
TNU 0:dfe498e03679 888 uint32_t val = 0;
TNU 0:dfe498e03679 889
TNU 0:dfe498e03679 890 uint16_t * u16ptr;
TNU 0:dfe498e03679 891 //pc->printf("Buffer space required: %d\n\r", MAXTRCLEN);
TNU 0:dfe498e03679 892 // Required buffer space
TNU 0:dfe498e03679 893 if (maxN < MAXTRCLEN){
TNU 0:dfe498e03679 894 pc-printf("NOT ENOUGH BUFFER SPACEn\r");
TNU 0:dfe498e03679 895 return -1;}
TNU 0:dfe498e03679 896
TNU 0:dfe498e03679 897 // Divide by payload size to get number of packets
TNU 0:dfe498e03679 898 // const int nPack = MAXTRCLEN / MLX_LONG_DATA_SZ;
TNU 0:dfe498e03679 899 // WTA: change nPack to a variable, initially 64
TNU 0:dfe498e03679 900 int nPack = MAXTRCLEN / MLX_LONG1_DATA_SZ;
TNU 0:dfe498e03679 901 //pc->printf("npack: %d", nPack);
TNU 0:dfe498e03679 902 // Ensure transmitted packet is all zeros to not send trash
TNU 0:dfe498e03679 903 memset(&txL, 0, sizeof(PACK_LONG1));
TNU 0:dfe498e03679 904
TNU 0:dfe498e03679 905 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 906 // memset(rx, 0, sizeof(rx));
TNU 0:dfe498e03679 907
TNU 0:dfe498e03679 908
TNU 0:dfe498e03679 909 res = ReadReg(336, &val); // PORT_CHSEL
TNU 0:dfe498e03679 910 if (res < 0)
TNU 0:dfe498e03679 911 goto END;
TNU 0:dfe498e03679 912 val >>= 16;
TNU 0:dfe498e03679 913 //pc->printf("chsel = %d\n", val);
TNU 0:dfe498e03679 914 cnt = 0;
TNU 0:dfe498e03679 915 // Count how many channels are selected
TNU 0:dfe498e03679 916 for (i = 0; i < 16; i++) {
TNU 0:dfe498e03679 917 if (val & 0x1)
TNU 0:dfe498e03679 918 cnt++;
TNU 0:dfe498e03679 919 val >>= 1;
TNU 0:dfe498e03679 920 }
TNU 0:dfe498e03679 921 nPack *= cnt;
TNU 0:dfe498e03679 922 nPack /= 16;
TNU 0:dfe498e03679 923
TNU 0:dfe498e03679 924 res = ReadReg(332, &val); // PORT_OVR_ACCUM_ACQ_OVR
TNU 0:dfe498e03679 925 // pc->printf("PORT_OVR = %d\n", (val >> 16));
TNU 0:dfe498e03679 926 val = (val >> 16) & 3; // Get bits 0 and 1
TNU 0:dfe498e03679 927
TNU 0:dfe498e03679 928 if (res < 0){
TNU 0:dfe498e03679 929 pc->printf("ReadReg Error1\n\r");
TNU 0:dfe498e03679 930 goto END;}
TNU 0:dfe498e03679 931
TNU 0:dfe498e03679 932 if (val == 0){ //00 = 1
TNU 0:dfe498e03679 933 nPack /= 4;
TNU 0:dfe498e03679 934 }
TNU 0:dfe498e03679 935 else if (val == 1){ //01 = 2
TNU 0:dfe498e03679 936 nPack /= 4;
TNU 0:dfe498e03679 937 }
TNU 0:dfe498e03679 938 else if (val == 2){ //10 = 4
TNU 0:dfe498e03679 939 nPack /= 2;
TNU 0:dfe498e03679 940 }
TNU 0:dfe498e03679 941 else if (val == 3){ //11 = 8
TNU 0:dfe498e03679 942 nPack /= 1;
TNU 0:dfe498e03679 943 }
TNU 0:dfe498e03679 944 else {
TNU 0:dfe498e03679 945 pc->printf("GetTrace: bad value\n");
TNU 0:dfe498e03679 946 }
TNU 0:dfe498e03679 947
TNU 0:dfe498e03679 948
TNU 0:dfe498e03679 949 // Write 1 to PORT_ACQU register and then wait
TNU 0:dfe498e03679 950 res = ReadReg(0x146, &val); // PORT_ACQU
TNU 0:dfe498e03679 951 if (res < 0){
TNU 0:dfe498e03679 952 pc->printf("ReadReg Error2\n\r");
TNU 0:dfe498e03679 953 goto END;}
TNU 0:dfe498e03679 954 val = (val>>16) | 1;
TNU 0:dfe498e03679 955 res = WriteReg(0x146, val); // PORT_ACQU
TNU 0:dfe498e03679 956 if (res < 0){
TNU 0:dfe498e03679 957 pc->printf("WriteReg Error3\n\r");
TNU 0:dfe498e03679 958 goto END;}
TNU 0:dfe498e03679 959
TNU 0:dfe498e03679 960 // Wait till PORT_READY bit is set.
TNU 0:dfe498e03679 961 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 962 cnt = 0;
TNU 0:dfe498e03679 963 while (((val & 0x10000) >> 16 != 1) && (cnt < 500)) {
TNU 0:dfe498e03679 964 wait_us(50);
TNU 0:dfe498e03679 965 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 966 cnt++;
TNU 0:dfe498e03679 967 }
TNU 0:dfe498e03679 968
TNU 0:dfe498e03679 969 // Encode the request and send it
TNU 0:dfe498e03679 970 res = MLX_ReqReadTrc(tx);
TNU 0:dfe498e03679 971 if (res < 0)
TNU 0:dfe498e03679 972 goto END;
TNU 0:dfe498e03679 973
TNU 0:dfe498e03679 974
TNU 0:dfe498e03679 975 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 976 //----SHORT PACKET EXCHANGE--------
TNU 0:dfe498e03679 977 res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), 0);
TNU 0:dfe498e03679 978 if (res < 0)
TNU 0:dfe498e03679 979 goto END;
TNU 0:dfe498e03679 980
TNU 0:dfe498e03679 981 pc->printf("MOSI ReqReadTrace:\t 0x");
TNU 0:dfe498e03679 982 for(int i=0;i<(sizeof(tx));i++){
TNU 0:dfe498e03679 983 uint8_t* pnt=(uint8_t*)tx;
TNU 0:dfe498e03679 984 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 985 }
TNU 0:dfe498e03679 986 pc->printf("\n");
TNU 0:dfe498e03679 987 pc->printf("MISO ReqReadTrace:\t 0x");
TNU 0:dfe498e03679 988 for(int i=0;i<(sizeof(tx));i++){
TNU 0:dfe498e03679 989 uint8_t* pnt=(uint8_t*)rx;
TNU 0:dfe498e03679 990 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 991 }
TNU 0:dfe498e03679 992 pc->printf("\n");
TNU 0:dfe498e03679 993 pc->printf("-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n");
TNU 0:dfe498e03679 994 //----PACKET EXCHANGE
TNU 0:dfe498e03679 995 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 996
TNU 0:dfe498e03679 997
TNU 0:dfe498e03679 998 // Optional status decoding
TNU 0:dfe498e03679 999 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 0:dfe498e03679 1000 if (res < 0 || iserr)
TNU 0:dfe498e03679 1001 goto END;
TNU 0:dfe498e03679 1002
TNU 0:dfe498e03679 1003
TNU 0:dfe498e03679 1004 // Temporary pointer to uint16 data, for simplicity purpose
TNU 0:dfe498e03679 1005 u16ptr = (uint16_t*)rxL.data;
TNU 0:dfe498e03679 1006 //device.format(16,1);
TNU 0:dfe498e03679 1007 trigger.write(1);
TNU 0:dfe498e03679 1008 for (a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 1009 {
TNU 0:dfe498e03679 1010 res = MLX_EncodeStatusL1(&txL, 0, 0);
TNU 0:dfe498e03679 1011 if (res < 0){
TNU 0:dfe498e03679 1012 res = -7;
TNU 0:dfe498e03679 1013 goto END;
TNU 0:dfe498e03679 1014 }
TNU 0:dfe498e03679 1015
TNU 0:dfe498e03679 1016 //Tools::Wait(10);
TNU 0:dfe498e03679 1017 wait_us(10);
TNU 0:dfe498e03679 1018
TNU 0:dfe498e03679 1019 // Clock the remaining long packets
TNU 0:dfe498e03679 1020 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG1));
TNU 0:dfe498e03679 1021 // 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 1022 // 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 1023 if (res < 0){
TNU 0:dfe498e03679 1024 res = -8;
TNU 0:dfe498e03679 1025 goto END;
TNU 0:dfe498e03679 1026 }
TNU 0:dfe498e03679 1027
TNU 0:dfe498e03679 1028
TNU 0:dfe498e03679 1029 // Decode the long responses, then extract data values
TNU 0:dfe498e03679 1030 res = MLX_DecodeResL1(&rxL);
TNU 0:dfe498e03679 1031 if ((res < 0)){
TNU 0:dfe498e03679 1032 pc->printf("LONG READ ERROR: Stopped at the %d long message\n", a);
TNU 0:dfe498e03679 1033 pc->printf("TXLONG MOSI Response: ");
TNU 0:dfe498e03679 1034 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 1035 uint8_t* pnt=(uint8_t*)(&txL);
TNU 0:dfe498e03679 1036 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 1037 }
TNU 0:dfe498e03679 1038 pc->printf("\n\r");
TNU 0:dfe498e03679 1039 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 1040 pc->printf("TXLONG MISO Response: ");
TNU 0:dfe498e03679 1041 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 1042 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 0:dfe498e03679 1043 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 1044
TNU 0:dfe498e03679 1045 }
TNU 0:dfe498e03679 1046 pc->printf("\n\r");
TNU 0:dfe498e03679 1047 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 1048 //printf("last RXL = \n");
TNU 0:dfe498e03679 1049 //for (i = 0; i < 300; i++) {
TNU 0:dfe498e03679 1050 // pc->printf("%02x", rxL.buf[i]);
TNU 0:dfe498e03679 1051 // if (((i + 1) % 8) == 0)
TNU 0:dfe498e03679 1052 // pc->printf("\n");
TNU 0:dfe498e03679 1053 //}
TNU 0:dfe498e03679 1054 //printf("\n");
TNU 0:dfe498e03679 1055 res = -9;
TNU 0:dfe498e03679 1056 goto END;
TNU 0:dfe498e03679 1057 }
TNU 0:dfe498e03679 1058
TNU 0:dfe498e03679 1059 // Copy the returned data into the user buffer
TNU 0:dfe498e03679 1060 // WTA: only half the MLX_LONG_DATA_SZ because we copy 2 bytes at a time
TNU 0:dfe498e03679 1061 for (b = 0; b < (MLX_LONG1_DATA_SZ / 2); ++b)
TNU 0:dfe498e03679 1062 {
TNU 0:dfe498e03679 1063 //WTA: removed MLX_HDR_SZ
TNU 0:dfe498e03679 1064 buf[a*(MLX_LONG1_DATA_SZ / 2) + b] = u16ptr[b];
TNU 0:dfe498e03679 1065 }
TNU 0:dfe498e03679 1066 if(a<64){
TNU 0:dfe498e03679 1067 pc->printf("Trace packet %d MOSI: \n\t\t\t\t\t0x", a);
TNU 0:dfe498e03679 1068 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 1069 uint8_t* pnt=(uint8_t*)(&txL);
TNU 0:dfe498e03679 1070 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 1071 if(((i %30) ==0)&&(i>0))pc->printf("\n\t\t\t\t\t");
TNU 0:dfe498e03679 1072 }
TNU 0:dfe498e03679 1073 pc->printf("\n\r");
TNU 0:dfe498e03679 1074 pc->printf("Trace packet %d MISO: \n\t\t\t\t\t0x", a);
TNU 0:dfe498e03679 1075 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 1076 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 0:dfe498e03679 1077 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 1078 if(((i %30) ==0)&&(i>0))pc->printf("\n\t\t\t\t\t");
TNU 0:dfe498e03679 1079 }
TNU 0:dfe498e03679 1080 pc->printf("\n\r");
TNU 0:dfe498e03679 1081 pc->printf("-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 1082 }
TNU 0:dfe498e03679 1083
TNU 0:dfe498e03679 1084 }
TNU 0:dfe498e03679 1085 trigger.write(0);
TNU 0:dfe498e03679 1086 device.format(8,1);
TNU 0:dfe498e03679 1087
TNU 0:dfe498e03679 1088 END:
TNU 0:dfe498e03679 1089 device.format(8,1);
TNU 0:dfe498e03679 1090 return res;
TNU 0:dfe498e03679 1091 }
TNU 0:dfe498e03679 1092
TNU 0:dfe498e03679 1093
TNU 6:748062f3de21 1094 int LidarSpi::LoadPatch (const char *patch, Serial * pc){
TNU 6:748062f3de21 1095 int res=0;
TNU 0:dfe498e03679 1096 uint16_t rSz;
TNU 0:dfe498e03679 1097 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 1098 PACK_LONG2 rxL, txL;
TNU 0:dfe498e03679 1099 uint8_t iserr;
TNU 0:dfe498e03679 1100 uint16_t err;
TNU 0:dfe498e03679 1101 //uint32_t val = 0;
TNU 0:dfe498e03679 1102 uint16_t nWordsRemaining;
TNU 0:dfe498e03679 1103 uint16_t nPack;
TNU 6:748062f3de21 1104 uint16_t nWords;
TNU 6:748062f3de21 1105
TNU 7:c47612b25c77 1106 uint8_t memory[15000];
TNU 6:748062f3de21 1107 uint16_t addrStart, startLine, nBytes ;
TNU 6:748062f3de21 1108 startLine=0;
TNU 7:c47612b25c77 1109 memset(memory, 0, 15000);
TNU 6:748062f3de21 1110
TNU 7:c47612b25c77 1111 int count=0;
TNU 6:748062f3de21 1112 res=WriteReg(0x1d8, 128);
TNU 6:748062f3de21 1113 if (res<0){
TNU 6:748062f3de21 1114 pc->printf("PORT_LONG_PACKET_SIZE Write error\n\r");
TNU 0:dfe498e03679 1115 goto END;
TNU 6:748062f3de21 1116 }
TNU 6:748062f3de21 1117
TNU 7:c47612b25c77 1118 res=WriteReg(0x6c, 31796); //0x7C19
TNU 7:c47612b25c77 1119 if (res<0) {
TNU 7:c47612b25c77 1120 pc->printf("REG_MLX16_ITC_MASK0 Write error\n\r");
TNU 7:c47612b25c77 1121 }
TNU 7:c47612b25c77 1122
TNU 6:748062f3de21 1123 wait_us(5);
TNU 6:748062f3de21 1124 pc->printf("Start loading fragments \n\r");
TNU 7:c47612b25c77 1125
TNU 7:c47612b25c77 1126 LoadPatchFragment(patch,&addrStart, &startLine, &nBytes, memory, pc);
TNU 7:c47612b25c77 1127 Trigger(0);
TNU 7:c47612b25c77 1128 pc->printf("Load fragment %d\n\r", count);
TNU 7:c47612b25c77 1129 for(int i =0; i<nBytes;i++){
TNU 7:c47612b25c77 1130 //pc->printf("Addr[%d] : %d\n\r", addrStart+i, memory[i] );
TNU 7:c47612b25c77 1131 }
TNU 7:c47612b25c77 1132 pc->printf("addrStart: %d, startLine %d, nBytes: %d\n\r",addrStart, startLine, nBytes);
TNU 7:c47612b25c77 1133 nWords= nBytes / 2;
TNU 6:748062f3de21 1134
TNU 7:c47612b25c77 1135 // Ensure all packets are all zeros to not send trash
TNU 7:c47612b25c77 1136 memset(tx, 0, sizeof(tx));
TNU 7:c47612b25c77 1137 memset(&rxL, 0, sizeof(rxL));
TNU 7:c47612b25c77 1138
TNU 7:c47612b25c77 1139 //The Number of words to be transferred can be a multiple of LONG_DATA_SZ. Incase it is not
TNU 7:c47612b25c77 1140 //multiple of LONG_DATA_SZ, we do last packet with partial data and remaining words as 0
TNU 7:c47612b25c77 1141 nPack = nWords / (MLX_LONG2_DATA_SZ / 2) + (nWords % (MLX_LONG2_DATA_SZ / 2) == 0 ? 0 :1);
TNU 7:c47612b25c77 1142 pc->printf("npack: %d\n\r", nPack);
TNU 7:c47612b25c77 1143 // Encode the request and send it
TNU 7:c47612b25c77 1144 res = MLX_ReqWriteFW(tx, nPack, addrStart);
TNU 7:c47612b25c77 1145 if (res < 0){
TNU 7:c47612b25c77 1146 pc->printf("Err @ 1");
TNU 7:c47612b25c77 1147 goto END; }
TNU 7:c47612b25c77 1148
TNU 7:c47612b25c77 1149 wait_us(16);
TNU 7:c47612b25c77 1150 res = TxPacket((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx));
TNU 7:c47612b25c77 1151 if (res < 0){
TNU 7:c47612b25c77 1152 pc->printf("Err @ 2");
TNU 7:c47612b25c77 1153 goto END; }
TNU 7:c47612b25c77 1154 //Trigger(1);
TNU 7:c47612b25c77 1155
TNU 7:c47612b25c77 1156 pc->printf("ReqWriteFirmware MOSI: \n\r0x");
TNU 7:c47612b25c77 1157 for(int k=0;k<(sizeof(tx));k++){
TNU 7:c47612b25c77 1158 uint8_t* pnt=(uint8_t*)(&tx);
TNU 7:c47612b25c77 1159 pc->printf("%02X", *(pnt+k));
TNU 7:c47612b25c77 1160 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 7:c47612b25c77 1161 }
TNU 7:c47612b25c77 1162 pc->printf("\n\r");
TNU 7:c47612b25c77 1163 pc->printf("ReqWriteFirmware MISO: \n\r0x");
TNU 7:c47612b25c77 1164 for(int k=0;k<(sizeof(rx));k++){
TNU 7:c47612b25c77 1165 uint8_t* pnt=(uint8_t*)(&rx);
TNU 7:c47612b25c77 1166 pc->printf("%02X", *(pnt+k));
TNU 7:c47612b25c77 1167 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 7:c47612b25c77 1168 }
TNU 7:c47612b25c77 1169 pc->printf("\n\r-------------------\n\r");
TNU 7:c47612b25c77 1170
TNU 7:c47612b25c77 1171
TNU 7:c47612b25c77 1172
TNU 7:c47612b25c77 1173 wait_us(5);
TNU 7:c47612b25c77 1174
TNU 7:c47612b25c77 1175 // Optional status decoding
TNU 7:c47612b25c77 1176 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 7:c47612b25c77 1177 if (res < 0 || iserr){
TNU 7:c47612b25c77 1178 pc->printf("Err @ 3");
TNU 7:c47612b25c77 1179
TNU 7:c47612b25c77 1180 pc->printf("LOAD PATCH REQ response MISO: \n\r0x");
TNU 7:c47612b25c77 1181 for(int k=0;k<(sizeof(PACK_LONG2));k++){
TNU 7:c47612b25c77 1182 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 7:c47612b25c77 1183 pc->printf("%02X", *(pnt+k));
TNU 7:c47612b25c77 1184 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 7:c47612b25c77 1185 }
TNU 7:c47612b25c77 1186 pc->printf("\n\r");
TNU 7:c47612b25c77 1187
TNU 7:c47612b25c77 1188
TNU 7:c47612b25c77 1189 goto END; }
TNU 7:c47612b25c77 1190
TNU 7:c47612b25c77 1191 nWordsRemaining = nWords;
TNU 7:c47612b25c77 1192 for (uint a = 0; a < nPack; ++a)
TNU 7:c47612b25c77 1193 {
TNU 7:c47612b25c77 1194 uint16_t size;
TNU 7:c47612b25c77 1195 if (nWordsRemaining > (MLX_LONG2_DATA_SZ / 2))
TNU 7:c47612b25c77 1196 size = MLX_LONG2_DATA_SZ / 2;
TNU 7:c47612b25c77 1197 else
TNU 7:c47612b25c77 1198 size = nWordsRemaining;
TNU 7:c47612b25c77 1199
TNU 7:c47612b25c77 1200 res = MLX_WriteDataL2(&txL, size, a, &memory[a*MLX_LONG2_DATA_SZ]);
TNU 6:748062f3de21 1201 if (res < 0){
TNU 7:c47612b25c77 1202 res = -7;
TNU 7:c47612b25c77 1203 goto END;
TNU 7:c47612b25c77 1204 }
TNU 7:c47612b25c77 1205
TNU 7:c47612b25c77 1206 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
TNU 7:c47612b25c77 1207
TNU 7:c47612b25c77 1208 //Trigger(0);
TNU 7:c47612b25c77 1209 wait_us(12);
TNU 7:c47612b25c77 1210 /*
TNU 6:748062f3de21 1211
TNU 7:c47612b25c77 1212 pc->printf("LOAD PATCH LONG RESPONSE %d of fragment %d MOSI: \n\r0x", a,count);
TNU 6:748062f3de21 1213 for(int k=0;k<(sizeof(PACK_LONG2));k++){
TNU 6:748062f3de21 1214 uint8_t* pnt=(uint8_t*)(&txL);
TNU 6:748062f3de21 1215 pc->printf("%02X", *(pnt+k));
TNU 6:748062f3de21 1216 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 6:748062f3de21 1217 }
TNU 6:748062f3de21 1218 pc->printf("\n\r");
TNU 7:c47612b25c77 1219 pc->printf("LOAD PATCH LONG RESPONSE %d of fragment %d MISO: \n\r0x", a,count);
TNU 6:748062f3de21 1220 for(int k=0;k<(sizeof(PACK_LONG2));k++){
TNU 6:748062f3de21 1221 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 6:748062f3de21 1222 pc->printf("%02X", *(pnt+k));
TNU 6:748062f3de21 1223 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 6:748062f3de21 1224 }
TNU 7:c47612b25c77 1225 pc->printf("\n\r-----------\n\r");
TNU 7:c47612b25c77 1226
TNU 6:748062f3de21 1227 */
TNU 6:748062f3de21 1228
TNU 7:c47612b25c77 1229 res = MLX_DecodeResL2(&rxL);
TNU 7:c47612b25c77 1230 if (res < 0){
TNU 7:c47612b25c77 1231 pc->printf("LONG WRITE ERROR: Stopped at the %d long message, res=%d\n", a, res);
TNU 6:748062f3de21 1232
TNU 7:c47612b25c77 1233 pc->printf("LOAD PATCH LONG RESPONSE %d MOSI: \n\r0x", a);
TNU 6:748062f3de21 1234 for(int k=0;k<(sizeof(PACK_LONG2));k++){
TNU 6:748062f3de21 1235 uint8_t* pnt=(uint8_t*)(&txL);
TNU 6:748062f3de21 1236 pc->printf("%02X", *(pnt+k));
TNU 6:748062f3de21 1237 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 6:748062f3de21 1238 }
TNU 6:748062f3de21 1239 pc->printf("\n\r");
TNU 7:c47612b25c77 1240 pc->printf("LOAD PATCH LONG RESPONSE %d MISO: \n\r0x", a);
TNU 6:748062f3de21 1241 for(int k=0;k<(sizeof(PACK_LONG2));k++){
TNU 6:748062f3de21 1242 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 6:748062f3de21 1243 pc->printf("%02X", *(pnt+k));
TNU 6:748062f3de21 1244 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 6:748062f3de21 1245 }
TNU 7:c47612b25c77 1246 pc->printf("\n\r");
TNU 7:c47612b25c77 1247 res = -9;
TNU 0:dfe498e03679 1248 goto END;
TNU 0:dfe498e03679 1249 }
TNU 7:c47612b25c77 1250
TNU 7:c47612b25c77 1251 nWordsRemaining = nWords - size;
TNU 6:748062f3de21 1252
TNU 7:c47612b25c77 1253 }
TNU 7:c47612b25c77 1254 count++;
TNU 7:c47612b25c77 1255 //LAST STATUS LONG PACKET FROM 75320 to get status of last write long
TNU 7:c47612b25c77 1256 res = MLX_EncodeStatusL2(&txL, 0, 0);
TNU 7:c47612b25c77 1257 if (res < 0) {
TNU 7:c47612b25c77 1258 res = -7;
TNU 7:c47612b25c77 1259 goto END;
TNU 6:748062f3de21 1260 }
TNU 7:c47612b25c77 1261
TNU 7:c47612b25c77 1262 // Clock the remaining long packets
TNU 7:c47612b25c77 1263 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
TNU 7:c47612b25c77 1264 wait_us(11);
TNU 7:c47612b25c77 1265 if (res < 0){
TNU 7:c47612b25c77 1266 pc->printf("Err @ 4");
TNU 7:c47612b25c77 1267 goto END; }
TNU 7:c47612b25c77 1268
TNU 7:c47612b25c77 1269
TNU 7:c47612b25c77 1270
TNU 7:c47612b25c77 1271
TNU 7:c47612b25c77 1272 memset(memory, 0, 512);
TNU 7:c47612b25c77 1273 wait_us(12);
TNU 7:c47612b25c77 1274
TNU 7:c47612b25c77 1275
TNU 6:748062f3de21 1276 pc->printf("Status long packet to check status after last fragment : %d, res=%d\n", count, res);
TNU 6:748062f3de21 1277
TNU 6:748062f3de21 1278 pc->printf("LOAD PATCH LONG STATUS %d MOSI: \n\r0x", count);
TNU 6:748062f3de21 1279 for(int k=0;k<(sizeof(PACK_LONG2));k++){
TNU 6:748062f3de21 1280 uint8_t* pnt=(uint8_t*)(&txL);
TNU 6:748062f3de21 1281 pc->printf("%02X", *(pnt+k));
TNU 6:748062f3de21 1282 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 0:dfe498e03679 1283 }
TNU 6:748062f3de21 1284 pc->printf("\n\r");
TNU 6:748062f3de21 1285 pc->printf("LOAD PATCH LONG RESPONSE %d MISO: \n\r0x", count);
TNU 6:748062f3de21 1286 for(int k=0;k<(sizeof(PACK_LONG2));k++){
TNU 6:748062f3de21 1287 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 6:748062f3de21 1288 pc->printf("%02X", *(pnt+k));
TNU 6:748062f3de21 1289 if(((k %30) ==0)&&(k>0)) pc->printf("\n\r");
TNU 0:dfe498e03679 1290 }
TNU 6:748062f3de21 1291 pc->printf("\n\r-----------\n\r");
TNU 7:c47612b25c77 1292
TNU 0:dfe498e03679 1293 // Change jump table pointer.
TNU 6:748062f3de21 1294 //res=LoadPatchFragment(patch,&addrStart, &startLine, &nBytes, memory, pc);
TNU 6:748062f3de21 1295 //pc->printf("Return from a loadPatchFragment: res = %d \n\r", res);
TNU 6:748062f3de21 1296 wait_ms(100);
TNU 6:748062f3de21 1297
TNU 7:c47612b25c77 1298 if (true){
TNU 6:748062f3de21 1299 pc->printf("Change jumptable... \n\r");
TNU 6:748062f3de21 1300 res = WriteReg(0x1000, 0x7000, pc); // write addr: 0x1000 value:0x7000
TNU 6:748062f3de21 1301 if (res < 0){
TNU 6:748062f3de21 1302 pc->printf("Err @ 5");
TNU 6:748062f3de21 1303 goto END;
TNU 6:748062f3de21 1304 }
TNU 6:748062f3de21 1305 }
TNU 6:748062f3de21 1306 else pc->printf("Failed to read the file\n\r");
TNU 0:dfe498e03679 1307
TNU 0:dfe498e03679 1308 END:
TNU 0:dfe498e03679 1309 return res;
TNU 0:dfe498e03679 1310 }
TNU 0:dfe498e03679 1311
TNU 0:dfe498e03679 1312 void LidarSpi::Trigger(int level){
TNU 0:dfe498e03679 1313 trigger.write(level);
TNU 0:dfe498e03679 1314 }
TNU 6:748062f3de21 1315
TNU 6:748062f3de21 1316
TNU 6:748062f3de21 1317 int LidarSpi::LoadPatchFragment(const char *patch, uint16_t *addrStart, uint16_t *startLine, uint16_t *nBytes, uint8_t *memory, Serial* pc){
TNU 6:748062f3de21 1318 char line[100];
TNU 6:748062f3de21 1319 FILE *fin;
TNU 6:748062f3de21 1320 uint16_t addr, n, status;
TNU 6:748062f3de21 1321 uint8_t bytes[256];
TNU 6:748062f3de21 1322 uint16_t i, total = 0, lineno = 1;
TNU 6:748062f3de21 1323 uint16_t minaddr = 65535, maxaddr = 0;
TNU 6:748062f3de21 1324
TNU 6:748062f3de21 1325 uint16_t prevAddr = 0;
TNU 6:748062f3de21 1326 uint16_t lineCount = *startLine;
TNU 6:748062f3de21 1327 uint16_t lines = 0;
TNU 6:748062f3de21 1328
TNU 6:748062f3de21 1329 bool isFirst = true;
TNU 6:748062f3de21 1330
TNU 6:748062f3de21 1331 *nBytes = 0;
TNU 6:748062f3de21 1332 if (strlen(patch) == 0) {
TNU 6:748062f3de21 1333 //printf(" Can't load a file without the filename.");
TNU 6:748062f3de21 1334 return -9;
TNU 6:748062f3de21 1335 }
TNU 6:748062f3de21 1336 fin = fopen(patch, "r");
TNU 6:748062f3de21 1337 if (fin == NULL) {
TNU 6:748062f3de21 1338 pc->printf(" Can't open file '%s' for reading.\n\r", patch);
TNU 6:748062f3de21 1339 return -10;
TNU 6:748062f3de21 1340 }
TNU 7:c47612b25c77 1341 pc->printf("Opened file\n\r");
TNU 6:748062f3de21 1342 // printf("Patch file %s opened\n", filename);
TNU 6:748062f3de21 1343
TNU 6:748062f3de21 1344 while (true) {
TNU 6:748062f3de21 1345 line[0] = '\0';
TNU 7:c47612b25c77 1346 fgets(line, 1000, fin);
TNU 7:c47612b25c77 1347 lines++;
TNU 6:748062f3de21 1348 //pc->printf("Startline: %d, lines: %d\n\r", *startLine,lines);
TNU 6:748062f3de21 1349
TNU 6:748062f3de21 1350 if (line[strlen(line) - 1] == '\n') line[strlen(line) - 1] = '\0';
TNU 6:748062f3de21 1351 if (line[strlen(line) - 1] == '\r') line[strlen(line) - 1] = '\0';
TNU 6:748062f3de21 1352 *addrStart= 0;
TNU 7:c47612b25c77 1353 //int res=parse_hex_line(line, bytes, &addr, &n, &status);
TNU 7:c47612b25c77 1354 //pc->printf("Result of parse: %d\n\r", res);
TNU 7:c47612b25c77 1355 if (parse_hex_line(line, bytes, &addr, &n, &status)>0) {
TNU 6:748062f3de21 1356 //cout << "Parse OK\n\r";
TNU 6:748062f3de21 1357 //pc->printf("Line: %s\n\r", line);
TNU 7:c47612b25c77 1358
TNU 7:c47612b25c77 1359 if (*nBytes>15000) {
TNU 6:748062f3de21 1360 *addrStart = minaddr;
TNU 6:748062f3de21 1361 *startLine = lines;
TNU 6:748062f3de21 1362 fclose(fin);
TNU 7:c47612b25c77 1363 pc->printf("Close file\n\r");
TNU 6:748062f3de21 1364 return -5;
TNU 6:748062f3de21 1365 }
TNU 6:748062f3de21 1366
TNU 6:748062f3de21 1367 if (status == 0) { /* data */
TNU 6:748062f3de21 1368 //cout << "Status=0";
TNU 7:c47612b25c77 1369 //if (addr < minaddr) minaddr = addr;
TNU 7:c47612b25c77 1370 //pc->printf("addr: %d, minaddr: %d\n\r", addr, minaddr);
TNU 6:748062f3de21 1371 isFirst = false;
TNU 7:c47612b25c77 1372 if (addr < minaddr) minaddr = addr;
TNU 6:748062f3de21 1373 for (i = 0; i <= (n - 1); i++) {
TNU 7:c47612b25c77 1374
TNU 7:c47612b25c77 1375
TNU 7:c47612b25c77 1376 prevAddr = addr;
TNU 7:c47612b25c77 1377
TNU 6:748062f3de21 1378 if (i % 2 == 0)
TNU 7:c47612b25c77 1379 memory[addr-minaddr] = bytes[i + 1] & 255; //Assumption even no of bytes per line
TNU 6:748062f3de21 1380 else
TNU 7:c47612b25c77 1381 memory[addr-minaddr] = bytes[i - 1] & 255;
TNU 7:c47612b25c77 1382
TNU 6:748062f3de21 1383 if (addr < minaddr) minaddr = addr;
TNU 7:c47612b25c77 1384 if ((addr) > maxaddr) maxaddr = (addr);
TNU 6:748062f3de21 1385 addr++;
TNU 7:c47612b25c77 1386 total++;
TNU 6:748062f3de21 1387 }
TNU 6:748062f3de21 1388 }
TNU 6:748062f3de21 1389 if (status == 1) { /* end of file */
TNU 6:748062f3de21 1390 fclose(fin);
TNU 7:c47612b25c77 1391 pc->printf("Closed file\n\r");
TNU 6:748062f3de21 1392 //printf("load_file parsed %d bytes between:", total);
TNU 6:748062f3de21 1393 //printf(" %04X to %04X\n", minaddr, maxaddr);
TNU 7:c47612b25c77 1394
TNU 6:748062f3de21 1395 *addrStart= minaddr;
TNU 7:c47612b25c77 1396 pc->printf("minAddr: %d, maxAddr: %d\n\r", minaddr, maxaddr);
TNU 7:c47612b25c77 1397 *nBytes=maxaddr-minaddr;
TNU 6:748062f3de21 1398 pc->printf("End of file\n\r");
TNU 6:748062f3de21 1399 return total;
TNU 6:748062f3de21 1400 }
TNU 6:748062f3de21 1401 if (status == 2) /* begin of file */
TNU 6:748062f3de21 1402 goto NEXT;
TNU 7:c47612b25c77 1403 *nBytes =0;
TNU 6:748062f3de21 1404 }
TNU 6:748062f3de21 1405 else {
TNU 7:c47612b25c77 1406 printf(" Error: '%s', line: %d\n\r", patch, lineno);
TNU 6:748062f3de21 1407 }
TNU 6:748062f3de21 1408 lineCount++;
TNU 6:748062f3de21 1409 NEXT: lineno++;
TNU 6:748062f3de21 1410 }
TNU 6:748062f3de21 1411 fclose(fin);
TNU 7:c47612b25c77 1412 pc->printf("Close file\n\r");
TNU 6:748062f3de21 1413 return 0;
TNU 6:748062f3de21 1414
TNU 6:748062f3de21 1415 }
TNU 6:748062f3de21 1416 int LidarSpi::parse_hex_line(char *theline, uint8_t bytes[], uint16_t *addr, uint16_t *num, uint16_t *code){
TNU 6:748062f3de21 1417 unsigned int sum, len, cksum;
TNU 6:748062f3de21 1418 unsigned int newAddr, newCode, newByte;
TNU 6:748062f3de21 1419
TNU 7:c47612b25c77 1420
TNU 6:748062f3de21 1421 char *ptr;
TNU 6:748062f3de21 1422 //cout << "Start parsing\n\r";
TNU 6:748062f3de21 1423 *num = 0;
TNU 6:748062f3de21 1424 if (theline[0] != ':') return 0;
TNU 6:748062f3de21 1425 if (strlen(theline) < 11) return 0;
TNU 6:748062f3de21 1426 ptr = theline + 1;
TNU 6:748062f3de21 1427 if (!sscanf(ptr, "%02x", &len)) return 0;
TNU 6:748062f3de21 1428 ptr += 2;
TNU 6:748062f3de21 1429 if (strlen(theline) < (11 + (len * 2))) return 0;
TNU 6:748062f3de21 1430 if (!sscanf(ptr, "%04x", &newAddr)) return 0;
TNU 6:748062f3de21 1431 *addr=(uint16_t)newAddr;
TNU 6:748062f3de21 1432 ptr += 4;
TNU 6:748062f3de21 1433 //printf("Line: length=%d Addr=%d\n", len, *addr);
TNU 6:748062f3de21 1434 if (!sscanf(ptr, "%02x", &newCode)) return 0;
TNU 7:c47612b25c77 1435
TNU 6:748062f3de21 1436 *code=(uint16_t)newCode;
TNU 6:748062f3de21 1437 ptr += 2;
TNU 6:748062f3de21 1438 sum = (len & 255) + ((*addr >> 8) & 255) + (*addr & 255) + (*code & 255);
TNU 6:748062f3de21 1439 while (*num != len) {
TNU 6:748062f3de21 1440 if (!sscanf(ptr, "%02x", &newByte )) return 0;
TNU 6:748062f3de21 1441 bytes[*num]=(uint8_t)newByte;
TNU 6:748062f3de21 1442 ptr += 2;
TNU 6:748062f3de21 1443 sum += bytes[*num] & 255;
TNU 6:748062f3de21 1444 (*num)++;
TNU 6:748062f3de21 1445 if (*num >= 256) return 0;
TNU 6:748062f3de21 1446 }
TNU 6:748062f3de21 1447 if (!sscanf(ptr, "%02x", &cksum)) return 0;
TNU 6:748062f3de21 1448 if (((sum & 255) + (cksum & 255)) & 255) return 0; /* checksum error */
TNU 6:748062f3de21 1449 return 1;
TNU 6:748062f3de21 1450 }
TNU 7:c47612b25c77 1451
TNU 7:c47612b25c77 1452 int LidarSpi::setTrace()
TNU 7:c47612b25c77 1453 {
TNU 7:c47612b25c77 1454 uint16_t val=148;
TNU 7:c47612b25c77 1455 int res=0;
TNU 7:c47612b25c77 1456 //pc->printf("Write PORT_LONG_PACKET_SIZE: %d\n\r",val);
TNU 7:c47612b25c77 1457 //Trigger(1);
TNU 7:c47612b25c77 1458 res=WriteReg(0x1d8, val);
TNU 7:c47612b25c77 1459 //Trigger(0);
TNU 7:c47612b25c77 1460 if (res<0){
TNU 7:c47612b25c77 1461 //pc->printf("PORT_LONG_PACKET_SIZE Write error\n\r");
TNU 7:c47612b25c77 1462 return res;
TNU 7:c47612b25c77 1463 }
TNU 7:c47612b25c77 1464
TNU 7:c47612b25c77 1465 //pc->printf("Packet size: %d\n\r", val);
TNU 7:c47612b25c77 1466
TNU 7:c47612b25c77 1467 //pc->printf("Write REG_PORT_CHSEL: 0xFFFF\n\r");
TNU 7:c47612b25c77 1468 res=WriteReg(0x150, 0xffff);
TNU 7:c47612b25c77 1469 if (res<0) {
TNU 7:c47612b25c77 1470 //pc->printf("REG_PORT_CHSEL Write error\n\r");
TNU 7:c47612b25c77 1471 return res;
TNU 7:c47612b25c77 1472 }
TNU 7:c47612b25c77 1473 //pc->printf("Write REG_PORT_OVR_ACCUM: 0x4B3\n\r");
TNU 7:c47612b25c77 1474 res=WriteReg(0x14c, 1203); //0x4B3
TNU 7:c47612b25c77 1475 //res=WriteReg(0x14c, 0x0fff); //0x4B3
TNU 7:c47612b25c77 1476 //res=WriteReg(0x14c, 0b0011); //0x4B3
TNU 7:c47612b25c77 1477 if (res<0) {
TNU 7:c47612b25c77 1478 //pc->printf("REG_PORT_OVR_ACCUM Write error\n\r");
TNU 7:c47612b25c77 1479 return res;
TNU 7:c47612b25c77 1480 }
TNU 7:c47612b25c77 1481 //pc->printf("Write REG_PORT_TRIGGER_PERIOD: 0x3E8\n\r");
TNU 7:c47612b25c77 1482 res=WriteReg(0x148, 1000); //0x3e8
TNU 7:c47612b25c77 1483 if (res<0) {
TNU 7:c47612b25c77 1484 //pc->printf("REG_PORT_TRIGGER_PERIOD Write error\n\r");
TNU 7:c47612b25c77 1485 return res;
TNU 7:c47612b25c77 1486 }
TNU 7:c47612b25c77 1487
TNU 7:c47612b25c77 1488 //pc->printf("Write REG_PORT_MEAS_DELAY: 0x0000\n\r");
TNU 7:c47612b25c77 1489 res=WriteReg(0x32, 0);
TNU 7:c47612b25c77 1490 if (res<0) {
TNU 7:c47612b25c77 1491 //pc->printf("REG_PORT_MEAS_DELAY Write error\n\r");
TNU 7:c47612b25c77 1492 return res;
TNU 7:c47612b25c77 1493 }
TNU 7:c47612b25c77 1494 //pc->printf("Write REG_PORT_MODE_EN: 0x0000\n\r");
TNU 7:c47612b25c77 1495 res=WriteReg(0x30, 0x00);
TNU 7:c47612b25c77 1496 if (res<0) {
TNU 7:c47612b25c77 1497 //pc->printf("REG_PORT_MODE_EN Write error\n\r");
TNU 7:c47612b25c77 1498 return res;
TNU 7:c47612b25c77 1499 }
TNU 7:c47612b25c77 1500
TNU 7:c47612b25c77 1501 //pc->printf("Write REG_MLX16_ITC_MASK0: 0x7C34\n\r");
TNU 7:c47612b25c77 1502 res=WriteReg(0x6c, 0x7C34); //0x7C19
TNU 7:c47612b25c77 1503 if (res<0) {
TNU 7:c47612b25c77 1504 //pc->printf("REG_MLX16_ITC_MASK0 Write error\n\r");
TNU 7:c47612b25c77 1505 return res;
TNU 7:c47612b25c77 1506 }
TNU 7:c47612b25c77 1507 //pc->printf("Write REG_STIMER2_VALUE: 0xBFFF\n\r");
TNU 7:c47612b25c77 1508 res=WriteReg(0x22, 49151); //0xBFFF
TNU 7:c47612b25c77 1509 if (res<0) {
TNU 7:c47612b25c77 1510 //pc->printf("REG_STIMER2_VALUE Write error\n\r");
TNU 7:c47612b25c77 1511 return res;
TNU 7:c47612b25c77 1512 }
TNU 7:c47612b25c77 1513 /*
TNU 7:c47612b25c77 1514 //pc->printf("Write REG_PORT_FIXED_DIVIDER: 0x802\n\r");
TNU 7:c47612b25c77 1515 res=WriteReg(0x14E, 0x0); //0x802
TNU 7:c47612b25c77 1516 if (res<0) {
TNU 7:c47612b25c77 1517 //pc->printf("REG_PORT_FIXED_DIVIDER Write error\n\r");
TNU 7:c47612b25c77 1518 return res;
TNU 7:c47612b25c77 1519 }
TNU 7:c47612b25c77 1520
TNU 7:c47612b25c77 1521 //0x124=0xae40, and 0x126=0xae40
TNU 7:c47612b25c77 1522 //pc->printf("Improvement registers for trace without patch\n\r");
TNU 7:c47612b25c77 1523 res=WriteReg(0x124, 0xae40); //0x802
TNU 7:c47612b25c77 1524 if (res<0) {
TNU 7:c47612b25c77 1525 //pc->printf("PORT SH1 Write error\n\r");
TNU 7:c47612b25c77 1526 return res;
TNU 7:c47612b25c77 1527 }
TNU 7:c47612b25c77 1528 res=WriteReg(0x126, 0xae40); //0x802
TNU 7:c47612b25c77 1529 if (res<0) {
TNU 7:c47612b25c77 1530 //pc->printf("PORT SH2 Write error\n\r");
TNU 7:c47612b25c77 1531 return res;
TNU 7:c47612b25c77 1532 }*/
TNU 7:c47612b25c77 1533 return 0;
TNU 7:c47612b25c77 1534
TNU 7:c47612b25c77 1535 }