Tom N / LidarSpi

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

Dependents:   MLX75320_API

LidarSpi.cpp@9:067f75510d67, 2016-04-15 (annotated)

Committer:
TNU
Date:
Fri Apr 15 13:53:31 2016 +0000
Revision:
9:067f75510d67
Parent:
8:d49102c10940
Child:
11:bd2dee9957a9
Updates

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