Tom N / LidarSpi

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

Dependents:   MLX75320_API

LidarSpi.cpp@11:bd2dee9957a9, 2016-04-15 (annotated)

Committer:
TNU
Date:
Fri Apr 15 14:07:49 2016 +0000
Revision:
11:bd2dee9957a9
Parent:
9:067f75510d67
Child:
12:d1767e2bd3a8
Fixed resetpin

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