Tom N / LidarSpi

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

Dependents:   MLX75320_API

LidarSpi.cpp@5:87e211a23654, 2016-03-04 (annotated)

Committer:
TNU
Date:
Fri Mar 04 14:17:32 2016 +0000
Revision:
5:87e211a23654
Parent:
3:9ed1d493c235
Child:
6:748062f3de21
Use dataReady pin

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