Tom N / LidarSpi

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

Dependents:   MLX75320_API

LidarSpi.cpp@12:d1767e2bd3a8, 2016-04-29 (annotated)

Committer:
TNU
Date:
Fri Apr 29 13:39:19 2016 +0000
Revision:
12:d1767e2bd3a8
Parent:
11:bd2dee9957a9
Child:
13:ccf4ab73c33d
General cleanup;

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