Tom N / LidarSpi

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

Dependents:   MLX75320_API

LidarSpi.cpp@1:e3ace426cee5, 2016-02-25 (annotated)

Committer:
TNU
Date:
Thu Feb 25 08:14:59 2016 +0000
Revision:
1:e3ace426cee5
Parent:
0:dfe498e03679
Child:
3:9ed1d493c235
getechos use full buffer, not half of it

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 0:dfe498e03679 9 LidarSpi::LidarSpi(PinName mosi, PinName miso, PinName clk, PinName chipSelect, PinName dr, PinName rs, PinName tr):device(mosi, miso, clk), chipS(chipSelect), dataReady(dr),resetPin(rs), trigger(tr)
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 0:dfe498e03679 144
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 0:dfe498e03679 217 return 0;
TNU 0:dfe498e03679 218 }
TNU 0:dfe498e03679 219
TNU 0:dfe498e03679 220 int LidarSpi::WriteReg(uint32_t reg, uint32_t val)
TNU 0:dfe498e03679 221 {
TNU 0:dfe498e03679 222 return WriteRegSpeed(reg, val, START_DELAY);
TNU 0:dfe498e03679 223 }
TNU 0:dfe498e03679 224
TNU 0:dfe498e03679 225 // Add a speed input so that we can control delay between packets
TNU 0:dfe498e03679 226 int LidarSpi::WriteRegSpeed(uint32_t reg, uint32_t val, uint16_t speed)
TNU 0:dfe498e03679 227 {
TNU 0:dfe498e03679 228 int res;
TNU 0:dfe498e03679 229 uint16_t rSz;
TNU 0:dfe498e03679 230 uint8_t iserr;
TNU 0:dfe498e03679 231 uint16_t error;
TNU 0:dfe498e03679 232 PACK_SHORT rx[2], tx[2];
TNU 0:dfe498e03679 233
TNU 0:dfe498e03679 234 // Ensure all packets are all zeros to not send trash
TNU 0:dfe498e03679 235 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 236
TNU 0:dfe498e03679 237 // Encode the request and send it
TNU 0:dfe498e03679 238 res = MLX_ReqWriteReg(tx, (uint32_t)reg, (uint32_t)val);
TNU 0:dfe498e03679 239 if (res < 0)
TNU 0:dfe498e03679 240 return res;
TNU 0:dfe498e03679 241
TNU 0:dfe498e03679 242 res = TxPacket((uint8_t*)rx, &rSz, ((uint8_t*)tx), sizeof(tx)/2);
TNU 0:dfe498e03679 243 wait_us(5);
TNU 0:dfe498e03679 244 res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2);
TNU 0:dfe498e03679 245 if (res < 0)
TNU 0:dfe498e03679 246 return res;
TNU 0:dfe498e03679 247
TNU 0:dfe498e03679 248
TNU 0:dfe498e03679 249
TNU 0:dfe498e03679 250 // Decode response (a status packet)
TNU 0:dfe498e03679 251 res = MLX_DecodeStatusS(rx + 1, &iserr, &error);
TNU 0:dfe498e03679 252 if (res < 0 || iserr)
TNU 0:dfe498e03679 253 return res;
TNU 0:dfe498e03679 254
TNU 0:dfe498e03679 255 return 0;
TNU 0:dfe498e03679 256 }
TNU 0:dfe498e03679 257
TNU 0:dfe498e03679 258 int LidarSpi::WriteReg(uint32_t reg, uint32_t val, Serial* pc)
TNU 0:dfe498e03679 259 {
TNU 0:dfe498e03679 260 return WriteRegSpeed(reg, val, START_DELAY, pc);
TNU 0:dfe498e03679 261 }
TNU 0:dfe498e03679 262
TNU 0:dfe498e03679 263 // Add a speed input so that we can control delay between packets
TNU 0:dfe498e03679 264 int LidarSpi::WriteRegSpeed(uint32_t reg, uint32_t val, uint16_t speed, Serial* pc)
TNU 0:dfe498e03679 265 {
TNU 0:dfe498e03679 266 int res;
TNU 0:dfe498e03679 267 uint16_t rSz;
TNU 0:dfe498e03679 268 uint8_t iserr;
TNU 0:dfe498e03679 269 uint16_t error;
TNU 0:dfe498e03679 270 PACK_SHORT rx[2], tx[2];
TNU 0:dfe498e03679 271
TNU 0:dfe498e03679 272 // Ensure all packets are all zeros to not send trash
TNU 0:dfe498e03679 273 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 274
TNU 0:dfe498e03679 275 // Encode the request and send it
TNU 0:dfe498e03679 276 res = MLX_ReqWriteReg(tx, (uint32_t)reg, (uint32_t)val);
TNU 0:dfe498e03679 277 if (res < 0)
TNU 0:dfe498e03679 278 return res;
TNU 0:dfe498e03679 279
TNU 0:dfe498e03679 280 /*
TNU 0:dfe498e03679 281 res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), speed);
TNU 0:dfe498e03679 282 if (res < 0)
TNU 0:dfe498e03679 283 return res;
TNU 0:dfe498e03679 284 */
TNU 0:dfe498e03679 285 res = TxPacket((uint8_t*)rx, &rSz, ((uint8_t*)tx), sizeof(tx)/2,pc);
TNU 0:dfe498e03679 286 wait_us(5);
TNU 0:dfe498e03679 287 res = TxPacket((uint8_t*)(rx+1), &rSz, (uint8_t*)(tx+1), sizeof(tx)/2, pc);
TNU 0:dfe498e03679 288 if (res < 0)
TNU 0:dfe498e03679 289 return -3;
TNU 0:dfe498e03679 290
TNU 0:dfe498e03679 291 //pc->printf("Write register request and response\n\r");
TNU 0:dfe498e03679 292
TNU 0:dfe498e03679 293 pc->printf("MOSI WriteRegRequest:\t 0x");
TNU 0:dfe498e03679 294 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 295 uint8_t* pnt=(uint8_t*)tx;
TNU 0:dfe498e03679 296 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 297 }
TNU 0:dfe498e03679 298 pc->printf("\n\r");
TNU 0:dfe498e03679 299 pc->printf("MISO 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*)rx;
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("-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 306
TNU 0:dfe498e03679 307
TNU 0:dfe498e03679 308 pc->printf("MOSI WriteRegResponse:\t 0x");
TNU 0:dfe498e03679 309 for(int i=0;i<(sizeof(tx)/2);i++){
TNU 0:dfe498e03679 310 uint8_t* pnt=(uint8_t*)(tx+1);
TNU 0:dfe498e03679 311 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 312 }
TNU 0:dfe498e03679 313 pc->printf("\n\r");
TNU 0:dfe498e03679 314 pc->printf("MISO 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*)(rx+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("==========================================================\n\r");
TNU 0:dfe498e03679 321
TNU 0:dfe498e03679 322
TNU 0:dfe498e03679 323
TNU 0:dfe498e03679 324 // Decode response (a status packet)
TNU 0:dfe498e03679 325 res = MLX_DecodeStatusS(rx + 1, &iserr, &error);
TNU 0:dfe498e03679 326 if (res < 0 || iserr){
TNU 0:dfe498e03679 327 pc->printf("Decode as short status: Result: %d, IsError: %d, Error: %d\n\r", res,iserr,error);
TNU 0:dfe498e03679 328 return res;}
TNU 0:dfe498e03679 329
TNU 0:dfe498e03679 330 return 0;
TNU 0:dfe498e03679 331 }
TNU 0:dfe498e03679 332 int LidarSpi::GetEchoes(Echo *ech, uint16_t maxN, uint16_t mode)
TNU 0:dfe498e03679 333 {
TNU 0:dfe498e03679 334 trigger.write(0);
TNU 0:dfe498e03679 335 int res, a, b;
TNU 0:dfe498e03679 336 uint16_t rSz;
TNU 0:dfe498e03679 337 uint32_t cnt;
TNU 0:dfe498e03679 338 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 339 PACK_LONG2 rxL, txL;
TNU 0:dfe498e03679 340 uint8_t iserr;
TNU 0:dfe498e03679 341 uint16_t err;
TNU 0:dfe498e03679 342 uint16_t idx = 0;
TNU 0:dfe498e03679 343 uint32_t val = 0;
TNU 0:dfe498e03679 344 uint16_t buf[MAXCH*4*BYTES_PER_ECH/2];
TNU 0:dfe498e03679 345
TNU 0:dfe498e03679 346 uint16_t * u16ptr ;
TNU 0:dfe498e03679 347
TNU 0:dfe498e03679 348
TNU 0:dfe498e03679 349 // Required buffer space
TNU 0:dfe498e03679 350 if (maxN < MAXECH)
TNU 0:dfe498e03679 351 return -1;
TNU 0:dfe498e03679 352
TNU 0:dfe498e03679 353 // Echo data is transmitted in 128 word payload => PACK_LONG2
TNU 0:dfe498e03679 354 // Each echo is X bytes, divide by payload size to get number of packets
TNU 0:dfe498e03679 355 const int nEchoes = MAXCH * 4;
TNU 0:dfe498e03679 356 const int nPack = nEchoes*BYTES_PER_ECH/MLX_LONG2_DATA_SZ;
TNU 0:dfe498e03679 357 const int WORDS_PER_ECH = BYTES_PER_ECH/2;
TNU 0:dfe498e03679 358
TNU 0:dfe498e03679 359 // Ensure transmitted packet is all zeros to not send trash
TNU 0:dfe498e03679 360 memset(&txL, 0, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 361 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 362
TNU 0:dfe498e03679 363 //res = MLXSPI::SetConfig(0); //debug
TNU 0:dfe498e03679 364
TNU 0:dfe498e03679 365 // Write 1 to PORT_ACQU register and then wait
TNU 0:dfe498e03679 366 res = ReadReg(0x146, &val); // PORT_ACQU
TNU 0:dfe498e03679 367 if (res < 0)
TNU 0:dfe498e03679 368 goto END;
TNU 0:dfe498e03679 369 val = (val >> 16) | 1;
TNU 0:dfe498e03679 370 trigger.write(1);
TNU 0:dfe498e03679 371 res = WriteReg(0x146, val); // PORT_ACQU
TNU 0:dfe498e03679 372 if (res < 0)
TNU 0:dfe498e03679 373 goto END;
TNU 0:dfe498e03679 374
TNU 0:dfe498e03679 375 // Wait till PORT_READY bit is set.
TNU 0:dfe498e03679 376 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 377
TNU 0:dfe498e03679 378
TNU 0:dfe498e03679 379
TNU 0:dfe498e03679 380 cnt = 0;
TNU 0:dfe498e03679 381 while (((val & 0x10000) >> 16 != 1) && (cnt < 500)) {
TNU 0:dfe498e03679 382 wait_us(50);
TNU 0:dfe498e03679 383 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 384 cnt++;
TNU 0:dfe498e03679 385 }
TNU 0:dfe498e03679 386 trigger.write(0);
TNU 0:dfe498e03679 387 // Encode the request and send it
TNU 0:dfe498e03679 388 res = MLX_ReqReadEch(tx);
TNU 0:dfe498e03679 389 if (res < 0)
TNU 0:dfe498e03679 390 goto END;
TNU 0:dfe498e03679 391
TNU 0:dfe498e03679 392 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 393 //----SHORT PACKET EXCHANGE--------
TNU 0:dfe498e03679 394 res = TxPacketSlow((uint8*)rx, &rSz, (uint8*)tx, sizeof(tx), 4);
TNU 0:dfe498e03679 395 if (res < 0)
TNU 0:dfe498e03679 396 goto END;
TNU 0:dfe498e03679 397 //----PACKET EXCHANGE
TNU 0:dfe498e03679 398
TNU 0:dfe498e03679 399 // Optional status decoding
TNU 0:dfe498e03679 400 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 0:dfe498e03679 401 if (res < 0 || iserr)
TNU 0:dfe498e03679 402 goto END;
TNU 0:dfe498e03679 403
TNU 0:dfe498e03679 404 // Temporary pointer to uint16 data, for simplicity purpose
TNU 0:dfe498e03679 405 u16ptr = (uint16*)rxL.data;
TNU 0:dfe498e03679 406
TNU 0:dfe498e03679 407
TNU 0:dfe498e03679 408 res = MLX_EncodeStatusL2(&txL, 0, 0); //Create echo status packet
TNU 0:dfe498e03679 409 if (res < 0){
TNU 0:dfe498e03679 410 res = -7;
TNU 0:dfe498e03679 411 goto END;
TNU 0:dfe498e03679 412 }
TNU 0:dfe498e03679 413 for (a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 414 {
TNU 0:dfe498e03679 415
TNU 0:dfe498e03679 416 wait_us(10);
TNU 0:dfe498e03679 417 //Tools::Wait(10);
TNU 0:dfe498e03679 418 // Clock the remaining long packets
TNU 0:dfe498e03679 419 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 420 //----LONG PACKET EXCHANGE--------
TNU 0:dfe498e03679 421 res=TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 422 if (res < 0)
TNU 0:dfe498e03679 423 goto END;
TNU 0:dfe498e03679 424
TNU 0:dfe498e03679 425 // Decode the long responses, then extract data values
TNU 0:dfe498e03679 426 res = MLX_DecodeResL2(&rxL);
TNU 0:dfe498e03679 427 if (res < 0)
TNU 0:dfe498e03679 428 goto END;
TNU 0:dfe498e03679 429
TNU 0:dfe498e03679 430 // Gather all of the echo data in a buffer.
TNU 0:dfe498e03679 431 //for (b = 0; b < (MLX_LONG2_DATA_SZ / 2); ++b)
TNU 0:dfe498e03679 432 for (b = 0; b < (128 / 2); ++b)
TNU 0:dfe498e03679 433 {
TNU 0:dfe498e03679 434 //buf[a*(MLX_LONG2_DATA_SZ / 2) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte
TNU 0:dfe498e03679 435 buf[a*(128 / 2) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte
TNU 0:dfe498e03679 436 }
TNU 0:dfe498e03679 437 }
TNU 0:dfe498e03679 438
TNU 0:dfe498e03679 439 // Do something with the data... decode an echo, manage the empty echo case
TNU 0:dfe498e03679 440 for (b = 0; b < nEchoes; ++b) {
TNU 0:dfe498e03679 441
TNU 0:dfe498e03679 442 ech[idx].mDistance = buf[b*WORDS_PER_ECH + 1] << 16 | buf[b*WORDS_PER_ECH];
TNU 0:dfe498e03679 443 ech[idx].mAmplitude = buf[b*WORDS_PER_ECH + 3] << 16 | buf[b*WORDS_PER_ECH +2];
TNU 0:dfe498e03679 444 ech[idx].mBase = buf[b*WORDS_PER_ECH + 5] << 16 | buf[b*WORDS_PER_ECH + 4];
TNU 0:dfe498e03679 445 ech[idx].mMaxIndex = buf[b*WORDS_PER_ECH + 6];
TNU 0:dfe498e03679 446 ech[idx].mChannelIndex = (uint8) buf[b*WORDS_PER_ECH + 7];
TNU 0:dfe498e03679 447 ech[idx].mValid = buf[b*WORDS_PER_ECH + 7] >>8;
TNU 0:dfe498e03679 448 ech[idx].mAmplitudeLowScale = buf[b*WORDS_PER_ECH + 9] | buf[b*WORDS_PER_ECH + 8];
TNU 0:dfe498e03679 449 ech[idx].mSaturationWidth = buf[b*WORDS_PER_ECH + 11] | buf[b*WORDS_PER_ECH + 10];
TNU 0:dfe498e03679 450
TNU 0:dfe498e03679 451 ++idx;
TNU 0:dfe498e03679 452 }
TNU 0:dfe498e03679 453
TNU 0:dfe498e03679 454 res = idx;
TNU 0:dfe498e03679 455
TNU 0:dfe498e03679 456 END:
TNU 0:dfe498e03679 457
TNU 0:dfe498e03679 458 return res;
TNU 0:dfe498e03679 459 }
TNU 0:dfe498e03679 460
TNU 0:dfe498e03679 461
TNU 0:dfe498e03679 462 int LidarSpi::GetEchoes(Echo *ech, uint16_t maxN, uint16_t mode, Serial* pc)
TNU 0:dfe498e03679 463 {
TNU 0:dfe498e03679 464 trigger.write(0);
TNU 0:dfe498e03679 465 int res, a, b;
TNU 0:dfe498e03679 466 uint16_t rSz;
TNU 0:dfe498e03679 467 uint32_t cnt;
TNU 0:dfe498e03679 468 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 469 PACK_LONG2 rxL, txL;
TNU 0:dfe498e03679 470 uint8_t iserr;
TNU 0:dfe498e03679 471 uint16_t err;
TNU 0:dfe498e03679 472 uint16_t idx = 0;
TNU 0:dfe498e03679 473 uint32_t val = 0;
TNU 0:dfe498e03679 474 uint16_t buf[MAXCH*4*BYTES_PER_ECH/2];
TNU 0:dfe498e03679 475 memset(buf, 0, MAXCH*4*BYTES_PER_ECH/2);
TNU 0:dfe498e03679 476 uint16_t * u16ptr ;
TNU 0:dfe498e03679 477
TNU 0:dfe498e03679 478
TNU 0:dfe498e03679 479 // Required buffer space
TNU 0:dfe498e03679 480 if (maxN < MAXECH){
TNU 0:dfe498e03679 481 pc->printf("maxN too small\n\r");
TNU 0:dfe498e03679 482 return -1;
TNU 0:dfe498e03679 483 }
TNU 0:dfe498e03679 484
TNU 0:dfe498e03679 485 // Echo data is transmitted in 128 word payload => PACK_LONG2
TNU 0:dfe498e03679 486 // Each echo is X bytes, divide by payload size to get number of packets
TNU 0:dfe498e03679 487 const int nEchoes = MAXCH * 4;
TNU 0:dfe498e03679 488 const int nPack = nEchoes*BYTES_PER_ECH/MLX_LONG2_DATA_SZ;
TNU 0:dfe498e03679 489 const int WORDS_PER_ECH = BYTES_PER_ECH/2;
TNU 0:dfe498e03679 490
TNU 0:dfe498e03679 491 // Ensure transmitted packet is all zeros to not send trash
TNU 0:dfe498e03679 492 memset(&txL, 0, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 493 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 494
TNU 0:dfe498e03679 495 //res = MLXSPI::SetConfig(0); //debug
TNU 0:dfe498e03679 496
TNU 0:dfe498e03679 497 // Write 1 to PORT_ACQU register and then wait
TNU 0:dfe498e03679 498 pc->printf("\tRead PORT_ACQU\n\r");
TNU 0:dfe498e03679 499 res = ReadReg(0x146, &val); // PORT_ACQU
TNU 0:dfe498e03679 500 if (res < 0){
TNU 0:dfe498e03679 501 pc->printf("ReadReg Error\n\r");
TNU 0:dfe498e03679 502 goto END;}
TNU 0:dfe498e03679 503 val = (val >> 16) | 1;
TNU 0:dfe498e03679 504 pc->printf("\tWrite 1 to PORT_ACQU\n\r");
TNU 0:dfe498e03679 505 trigger.write(1);
TNU 0:dfe498e03679 506 res = WriteReg(0x146, val); // PORT_ACQU
TNU 0:dfe498e03679 507 if (res < 0){
TNU 0:dfe498e03679 508 pc->printf("WriteReg Error\n\r");
TNU 0:dfe498e03679 509 goto END;}
TNU 0:dfe498e03679 510
TNU 0:dfe498e03679 511 // Wait till PORT_READY bit is set.
TNU 0:dfe498e03679 512
TNU 0:dfe498e03679 513 pc->printf("\tWait for PORT_READY bit\n\r");
TNU 0:dfe498e03679 514 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 515 while(res<0){
TNU 0:dfe498e03679 516 pc->printf("Error: reread state of ready PIN\n\r");
TNU 0:dfe498e03679 517 wait_us(4);
TNU 0:dfe498e03679 518 res=ReadReg(470, &val);
TNU 0:dfe498e03679 519 }
TNU 0:dfe498e03679 520 cnt = 0;
TNU 0:dfe498e03679 521 while (((val & 0x10000) >> 16 != 1) && (cnt < 500)) {
TNU 0:dfe498e03679 522 wait_us(50);
TNU 0:dfe498e03679 523 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 524 while(res<0){
TNU 0:dfe498e03679 525 wait_us(4);
TNU 0:dfe498e03679 526 res = ReadReg(470, &val);
TNU 0:dfe498e03679 527 }
TNU 0:dfe498e03679 528 cnt++;
TNU 0:dfe498e03679 529 }
TNU 0:dfe498e03679 530 pc->printf("Counter: %d\n\r", cnt);
TNU 0:dfe498e03679 531
TNU 0:dfe498e03679 532 // Encode the request and send it
TNU 0:dfe498e03679 533 res = MLX_ReqReadEch(tx);
TNU 0:dfe498e03679 534 if (res < 0){
TNU 0:dfe498e03679 535 pc->printf("ReqreadEch error\n\r");
TNU 0:dfe498e03679 536 goto END;}
TNU 0:dfe498e03679 537
TNU 0:dfe498e03679 538 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 539 //----SHORT PACKET EXCHANGE--------
TNU 0:dfe498e03679 540 pc->printf("\tSend ReqReadEch\n\r");
TNU 0:dfe498e03679 541 res = TxPacketSlow((uint8*)rx, &rSz, (uint8*)tx, sizeof(tx), 4);
TNU 0:dfe498e03679 542 if (res < 0){
TNU 0:dfe498e03679 543 pc->printf("txPacketSlow Error\n\r");
TNU 0:dfe498e03679 544 goto END;}
TNU 0:dfe498e03679 545 /*
TNU 0:dfe498e03679 546 pc->printf("\tFirmware read request - processed\n\r");
TNU 0:dfe498e03679 547 pc->printf("Firmware readRequest MOSI: \t");
TNU 0:dfe498e03679 548 for(int i =0; i<sizeof(tx);i++) {
TNU 0:dfe498e03679 549 uint8_t* pnt=(uint8_t*)(&tx);
TNU 0:dfe498e03679 550 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 551 }
TNU 0:dfe498e03679 552 pc->printf("\n\r");
TNU 0:dfe498e03679 553 pc->printf("Firmware readRequest MISO: \t");
TNU 0:dfe498e03679 554 for(int i =0; i<sizeof(rx);i++) {
TNU 0:dfe498e03679 555 uint8_t* pnt=(uint8_t*)(&rx);
TNU 0:dfe498e03679 556 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 557 }
TNU 0:dfe498e03679 558 pc->printf("\n\r");
TNU 0:dfe498e03679 559 pc->printf("-- -- -- -- -- -- --\n\r");*/
TNU 0:dfe498e03679 560
TNU 0:dfe498e03679 561 //----PACKET EXCHANGE
TNU 0:dfe498e03679 562
TNU 0:dfe498e03679 563 // Optional status decoding
TNU 0:dfe498e03679 564 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 0:dfe498e03679 565 if (res < 0 || iserr){
TNU 0:dfe498e03679 566 pc->printf("Short status decode: Iss err?\n\r");
TNU 0:dfe498e03679 567 goto END;}
TNU 0:dfe498e03679 568
TNU 0:dfe498e03679 569 // Temporary pointer to uint16 data, for simplicity purpose
TNU 0:dfe498e03679 570 u16ptr = (uint16*)rxL.data;
TNU 0:dfe498e03679 571 PACK_LONG2 prevRX;
TNU 0:dfe498e03679 572 PACK_LONG2 prevTX;
TNU 0:dfe498e03679 573 res = MLX_EncodeStatusL2(&txL, 0, 0); //Create echo status packet
TNU 0:dfe498e03679 574 memset(&prevRX,0,sizeof(prevRX));
TNU 0:dfe498e03679 575 memset(&prevTX,0,sizeof(prevTX));
TNU 0:dfe498e03679 576 pc->printf("Value of npack:%d \n\r", nPack);
TNU 0:dfe498e03679 577 for (a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 578 {
TNU 0:dfe498e03679 579
TNU 0:dfe498e03679 580 if (res < 0){
TNU 0:dfe498e03679 581 res = -7;
TNU 0:dfe498e03679 582 pc->printf("Problem creating echo status\n\r");
TNU 0:dfe498e03679 583 goto END;
TNU 0:dfe498e03679 584 }
TNU 0:dfe498e03679 585 wait_us(10);
TNU 0:dfe498e03679 586 //Tools::Wait(10);
TNU 0:dfe498e03679 587 // Clock the remaining long packets
TNU 0:dfe498e03679 588 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 589 //----LONG PACKET EXCHANGE--------
TNU 0:dfe498e03679 590 res=TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 591 if (res < 0){
TNU 0:dfe498e03679 592 pc->printf("Packet #%d => txPacket_long error\n\r",a);
TNU 0:dfe498e03679 593 goto END;}
TNU 0:dfe498e03679 594
TNU 0:dfe498e03679 595
TNU 0:dfe498e03679 596 // Decode the long responses, then extract data values
TNU 0:dfe498e03679 597 res = MLX_DecodeResL2(&rxL);
TNU 1:e3ace426cee5 598 if ((res < 0)){
TNU 0:dfe498e03679 599
TNU 0:dfe498e03679 600 pc->printf("\n\r");
TNU 0:dfe498e03679 601 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 602 pc->printf("Packet #%d => Decode long response error \n\r", a);
TNU 0:dfe498e03679 603
TNU 0:dfe498e03679 604 pc->printf("TXLONG MOSI Response: ");
TNU 0:dfe498e03679 605 for(int i=0;i<(sizeof(PACK_LONG2));i++){
TNU 0:dfe498e03679 606 uint8_t* pnt=(uint8_t*)(&txL);
TNU 0:dfe498e03679 607 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 608 }
TNU 0:dfe498e03679 609 pc->printf("\n\r");
TNU 0:dfe498e03679 610 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 611 pc->printf("TXLONG MISO Response: ");
TNU 0:dfe498e03679 612 for(int i=0;i<(sizeof(PACK_LONG2));i++){
TNU 0:dfe498e03679 613 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 0:dfe498e03679 614 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 615
TNU 0:dfe498e03679 616 }
TNU 0:dfe498e03679 617 pc->printf("\n\r");
TNU 0:dfe498e03679 618 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 619 pc->printf("Packet #%d => Decode long response error \n\r", a);
TNU 0:dfe498e03679 620 goto END;}
TNU 0:dfe498e03679 621
TNU 0:dfe498e03679 622 // Gather all of the echo data in a buffer.
TNU 0:dfe498e03679 623 //for (b = 0; b < (MLX_LONG2_DATA_SZ / 2); ++b)
TNU 1:e3ace426cee5 624 for (b = 0; b < (128); ++b)
TNU 0:dfe498e03679 625 {
TNU 0:dfe498e03679 626 //buf[a*(MLX_LONG2_DATA_SZ / 2) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte
TNU 1:e3ace426cee5 627 buf[a*(128) + b] = (((u16ptr[b] & 0x00ff) << 8) | ((u16ptr[b] & 0xff00) >> 8)); //Swap LSByte with MSByte
TNU 0:dfe498e03679 628 }
TNU 0:dfe498e03679 629 //prevTX=txL;
TNU 0:dfe498e03679 630 //prevRX=rxL;
TNU 0:dfe498e03679 631 }
TNU 0:dfe498e03679 632
TNU 0:dfe498e03679 633 // Do something with the data... decode an echo, manage the empty echo case
TNU 0:dfe498e03679 634 for (b = 0; b < nEchoes; ++b) {
TNU 0:dfe498e03679 635
TNU 0:dfe498e03679 636 ech[idx].mDistance = buf[b*WORDS_PER_ECH + 1] << 16 | buf[b*WORDS_PER_ECH];
TNU 0:dfe498e03679 637 ech[idx].mAmplitude = buf[b*WORDS_PER_ECH + 3] << 16 | buf[b*WORDS_PER_ECH +2];
TNU 0:dfe498e03679 638 ech[idx].mBase = buf[b*WORDS_PER_ECH + 5] << 16 | buf[b*WORDS_PER_ECH + 4];
TNU 0:dfe498e03679 639 ech[idx].mMaxIndex = buf[b*WORDS_PER_ECH + 6];
TNU 0:dfe498e03679 640 ech[idx].mChannelIndex = (uint8) buf[b*WORDS_PER_ECH + 7];
TNU 0:dfe498e03679 641 ech[idx].mValid = buf[b*WORDS_PER_ECH + 7] >>8;
TNU 0:dfe498e03679 642 ech[idx].mAmplitudeLowScale = buf[b*WORDS_PER_ECH + 9] | buf[b*WORDS_PER_ECH + 8];
TNU 0:dfe498e03679 643 ech[idx].mSaturationWidth = buf[b*WORDS_PER_ECH + 11] | buf[b*WORDS_PER_ECH + 10];
TNU 0:dfe498e03679 644
TNU 0:dfe498e03679 645 ++idx;
TNU 0:dfe498e03679 646 }
TNU 0:dfe498e03679 647
TNU 0:dfe498e03679 648 res = idx;
TNU 0:dfe498e03679 649 trigger.write(0);
TNU 0:dfe498e03679 650
TNU 0:dfe498e03679 651 END:
TNU 0:dfe498e03679 652 trigger.write(0);
TNU 0:dfe498e03679 653 return res;
TNU 0:dfe498e03679 654 }
TNU 0:dfe498e03679 655
TNU 0:dfe498e03679 656
TNU 0:dfe498e03679 657
TNU 0:dfe498e03679 658
TNU 0:dfe498e03679 659 int LidarSpi::GetTrace ( uint16_t *buf, uint16_t maxN, uint16_t nSam, uint16_t idx, Serial* pc){
TNU 0:dfe498e03679 660 int res, a, b, i;
TNU 0:dfe498e03679 661 uint32_t cnt;
TNU 0:dfe498e03679 662 uint16_t rSz;
TNU 0:dfe498e03679 663 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 664 PACK_LONG1 rxL, txL;
TNU 0:dfe498e03679 665 uint8_t iserr;
TNU 0:dfe498e03679 666 uint16_t err;
TNU 0:dfe498e03679 667 uint32_t val = 0;
TNU 0:dfe498e03679 668
TNU 0:dfe498e03679 669 uint16_t * u16ptr;
TNU 0:dfe498e03679 670 pc->printf("Buffer space required: %d\n\r", MAXTRCLEN);
TNU 0:dfe498e03679 671 // Required buffer space
TNU 0:dfe498e03679 672 if (maxN < MAXTRCLEN){
TNU 0:dfe498e03679 673 pc-printf("NOT ENOUGH BUFFER SPACEn\r");
TNU 0:dfe498e03679 674 return -1;}
TNU 0:dfe498e03679 675
TNU 0:dfe498e03679 676 // Divide by payload size to get number of packets
TNU 0:dfe498e03679 677 // const int nPack = MAXTRCLEN / MLX_LONG_DATA_SZ;
TNU 0:dfe498e03679 678 // WTA: change nPack to a variable, initially 64
TNU 0:dfe498e03679 679 int nPack = MAXTRCLEN / MLX_LONG1_DATA_SZ;
TNU 0:dfe498e03679 680 pc->printf("npack: %d", nPack);
TNU 0:dfe498e03679 681 // Ensure transmitted packet is all zeros to not send trash
TNU 0:dfe498e03679 682 memset(&txL, 0, sizeof(PACK_LONG1));
TNU 0:dfe498e03679 683
TNU 0:dfe498e03679 684 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 685 // memset(rx, 0, sizeof(rx));
TNU 0:dfe498e03679 686
TNU 0:dfe498e03679 687
TNU 0:dfe498e03679 688 res = ReadReg(336, &val); // PORT_CHSEL
TNU 0:dfe498e03679 689 if (res < 0)
TNU 0:dfe498e03679 690 goto END;
TNU 0:dfe498e03679 691 val >>= 16;
TNU 0:dfe498e03679 692 pc->printf("chsel = %d\n", val);
TNU 0:dfe498e03679 693 cnt = 0;
TNU 0:dfe498e03679 694 // Count how many channels are selected
TNU 0:dfe498e03679 695 for (i = 0; i < 16; i++) {
TNU 0:dfe498e03679 696 if (val & 0x1)
TNU 0:dfe498e03679 697 cnt++;
TNU 0:dfe498e03679 698 val >>= 1;
TNU 0:dfe498e03679 699 }
TNU 0:dfe498e03679 700 nPack *= cnt;
TNU 0:dfe498e03679 701 nPack /= 16;
TNU 0:dfe498e03679 702
TNU 0:dfe498e03679 703 res = ReadReg(332, &val); // PORT_OVR_ACCUM_ACQ_OVR
TNU 0:dfe498e03679 704 // pc->printf("PORT_OVR = %d\n", (val >> 16));
TNU 0:dfe498e03679 705 val = (val >> 16) & 3; // Get bits 0 and 1
TNU 0:dfe498e03679 706
TNU 0:dfe498e03679 707 if (res < 0){
TNU 0:dfe498e03679 708 pc->printf("ReadReg Error1\n\r");
TNU 0:dfe498e03679 709 goto END;}
TNU 0:dfe498e03679 710
TNU 0:dfe498e03679 711 if (val == 0){ //00 = 1
TNU 0:dfe498e03679 712 nPack /= 4;
TNU 0:dfe498e03679 713 }
TNU 0:dfe498e03679 714 else if (val == 1){ //01 = 2
TNU 0:dfe498e03679 715 nPack /= 4;
TNU 0:dfe498e03679 716 }
TNU 0:dfe498e03679 717 else if (val == 2){ //10 = 4
TNU 0:dfe498e03679 718 nPack /= 2;
TNU 0:dfe498e03679 719 }
TNU 0:dfe498e03679 720 else if (val == 3){ //11 = 8
TNU 0:dfe498e03679 721 nPack /= 1;
TNU 0:dfe498e03679 722 }
TNU 0:dfe498e03679 723 else {
TNU 0:dfe498e03679 724 pc->printf("GetTrace: bad value\n");
TNU 0:dfe498e03679 725 }
TNU 0:dfe498e03679 726
TNU 0:dfe498e03679 727
TNU 0:dfe498e03679 728 // Write 1 to PORT_ACQU register and then wait
TNU 0:dfe498e03679 729 res = ReadReg(0x146, &val); // PORT_ACQU
TNU 0:dfe498e03679 730 if (res < 0){
TNU 0:dfe498e03679 731 pc->printf("ReadReg Error2\n\r");
TNU 0:dfe498e03679 732 goto END;}
TNU 0:dfe498e03679 733 val = (val>>16) | 1;
TNU 0:dfe498e03679 734 res = WriteReg(0x146, val); // PORT_ACQU
TNU 0:dfe498e03679 735 if (res < 0){
TNU 0:dfe498e03679 736 pc->printf("WriteReg Error3\n\r");
TNU 0:dfe498e03679 737 goto END;}
TNU 0:dfe498e03679 738
TNU 0:dfe498e03679 739 // Wait till PORT_READY bit is set.
TNU 0:dfe498e03679 740 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 741 cnt = 0;
TNU 0:dfe498e03679 742 while (((val & 0x10000) >> 16 != 1) && (cnt < 500)) {
TNU 0:dfe498e03679 743 wait_us(50);
TNU 0:dfe498e03679 744 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 745 cnt++;
TNU 0:dfe498e03679 746 }
TNU 0:dfe498e03679 747
TNU 0:dfe498e03679 748 // Encode the request and send it
TNU 0:dfe498e03679 749 res = MLX_ReqReadTrc(tx);
TNU 0:dfe498e03679 750 if (res < 0)
TNU 0:dfe498e03679 751 goto END;
TNU 0:dfe498e03679 752
TNU 0:dfe498e03679 753
TNU 0:dfe498e03679 754 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 755 //----SHORT PACKET EXCHANGE--------
TNU 0:dfe498e03679 756 res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), 0);
TNU 0:dfe498e03679 757 if (res < 0)
TNU 0:dfe498e03679 758 goto END;
TNU 0:dfe498e03679 759 //----PACKET EXCHANGE
TNU 0:dfe498e03679 760 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 761
TNU 0:dfe498e03679 762
TNU 0:dfe498e03679 763 // Optional status decoding
TNU 0:dfe498e03679 764 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 0:dfe498e03679 765 if (res < 0 || iserr)
TNU 0:dfe498e03679 766 goto END;
TNU 0:dfe498e03679 767
TNU 0:dfe498e03679 768
TNU 0:dfe498e03679 769 // Temporary pointer to uint16 data, for simplicity purpose
TNU 0:dfe498e03679 770 u16ptr = (uint16_t*)rxL.data;
TNU 0:dfe498e03679 771 //device.format(16,1);
TNU 0:dfe498e03679 772 trigger.write(1);
TNU 0:dfe498e03679 773 for (a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 774 {
TNU 0:dfe498e03679 775 res = MLX_EncodeStatusL1(&txL, 0, 0);
TNU 0:dfe498e03679 776 if (res < 0){
TNU 0:dfe498e03679 777 res = -7;
TNU 0:dfe498e03679 778 goto END;
TNU 0:dfe498e03679 779 }
TNU 0:dfe498e03679 780
TNU 0:dfe498e03679 781 //Tools::Wait(10);
TNU 0:dfe498e03679 782 wait_us(10);
TNU 0:dfe498e03679 783
TNU 0:dfe498e03679 784 // Clock the remaining long packets
TNU 0:dfe498e03679 785 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG1));
TNU 0:dfe498e03679 786 // 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 787 // 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 788 if (res < 0){
TNU 0:dfe498e03679 789 res = -8;
TNU 0:dfe498e03679 790 goto END;
TNU 0:dfe498e03679 791 }
TNU 0:dfe498e03679 792
TNU 0:dfe498e03679 793 // Decode the long responses, then extract data values
TNU 0:dfe498e03679 794 res = MLX_DecodeResL1(&rxL);
TNU 0:dfe498e03679 795 if ((res < 0)){
TNU 0:dfe498e03679 796 pc->printf("LONG READ ERROR: Stopped at the %d long message\n", a);
TNU 0:dfe498e03679 797 pc->printf("TXLONG MOSI Response: ");
TNU 0:dfe498e03679 798 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 799 uint8_t* pnt=(uint8_t*)(&txL);
TNU 0:dfe498e03679 800 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 801 }
TNU 0:dfe498e03679 802 pc->printf("\n\r");
TNU 0:dfe498e03679 803 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 804 pc->printf("TXLONG MISO Response: ");
TNU 0:dfe498e03679 805 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 806 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 0:dfe498e03679 807 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 808
TNU 0:dfe498e03679 809 }
TNU 0:dfe498e03679 810 pc->printf("\n\r");
TNU 0:dfe498e03679 811 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 812 //printf("last RXL = \n");
TNU 0:dfe498e03679 813 //for (i = 0; i < 300; i++) {
TNU 0:dfe498e03679 814 // pc->printf("%02x", rxL.buf[i]);
TNU 0:dfe498e03679 815 // if (((i + 1) % 8) == 0)
TNU 0:dfe498e03679 816 // pc->printf("\n");
TNU 0:dfe498e03679 817 //}
TNU 0:dfe498e03679 818 //printf("\n");
TNU 0:dfe498e03679 819 res = -9;
TNU 0:dfe498e03679 820 goto END;
TNU 0:dfe498e03679 821 }
TNU 0:dfe498e03679 822
TNU 0:dfe498e03679 823 // Copy the returned data into the user buffer
TNU 0:dfe498e03679 824 // WTA: only half the MLX_LONG_DATA_SZ because we copy 2 bytes at a time
TNU 0:dfe498e03679 825 for (b = 0; b < (MLX_LONG1_DATA_SZ / 2); ++b)
TNU 0:dfe498e03679 826 {
TNU 0:dfe498e03679 827 //WTA: removed MLX_HDR_SZ
TNU 0:dfe498e03679 828 buf[a*(MLX_LONG1_DATA_SZ / 2) + b] = u16ptr[b];
TNU 0:dfe498e03679 829 }
TNU 0:dfe498e03679 830
TNU 0:dfe498e03679 831 }
TNU 0:dfe498e03679 832 trigger.write(0);
TNU 0:dfe498e03679 833 device.format(8,1);
TNU 0:dfe498e03679 834
TNU 0:dfe498e03679 835 END:
TNU 0:dfe498e03679 836 device.format(8,1);
TNU 0:dfe498e03679 837 return res;
TNU 0:dfe498e03679 838 }
TNU 0:dfe498e03679 839
TNU 0:dfe498e03679 840 int LidarSpi::GetTraceOne ( uint16_t *buf, uint16_t maxN, uint16_t nSam, uint16_t idx,int index , Serial* pc){
TNU 0:dfe498e03679 841 int res, a, b, i;
TNU 0:dfe498e03679 842 uint32_t cnt;
TNU 0:dfe498e03679 843 uint16_t rSz;
TNU 0:dfe498e03679 844 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 845 PACK_LONG1 rxL, txL;
TNU 0:dfe498e03679 846 uint8_t iserr;
TNU 0:dfe498e03679 847 uint16_t err;
TNU 0:dfe498e03679 848 uint32_t val = 0;
TNU 0:dfe498e03679 849
TNU 0:dfe498e03679 850 uint16_t * u16ptr;
TNU 0:dfe498e03679 851 //pc->printf("Buffer space required: %d\n\r", MAXTRCLEN);
TNU 0:dfe498e03679 852 // Required buffer space
TNU 0:dfe498e03679 853 if (maxN < MAXTRCLEN){
TNU 0:dfe498e03679 854 pc-printf("NOT ENOUGH BUFFER SPACEn\r");
TNU 0:dfe498e03679 855 return -1;}
TNU 0:dfe498e03679 856
TNU 0:dfe498e03679 857 // Divide by payload size to get number of packets
TNU 0:dfe498e03679 858 // const int nPack = MAXTRCLEN / MLX_LONG_DATA_SZ;
TNU 0:dfe498e03679 859 // WTA: change nPack to a variable, initially 64
TNU 0:dfe498e03679 860 int nPack = MAXTRCLEN / MLX_LONG1_DATA_SZ;
TNU 0:dfe498e03679 861 //pc->printf("npack: %d", nPack);
TNU 0:dfe498e03679 862 // Ensure transmitted packet is all zeros to not send trash
TNU 0:dfe498e03679 863 memset(&txL, 0, sizeof(PACK_LONG1));
TNU 0:dfe498e03679 864
TNU 0:dfe498e03679 865 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 866 // memset(rx, 0, sizeof(rx));
TNU 0:dfe498e03679 867
TNU 0:dfe498e03679 868
TNU 0:dfe498e03679 869 res = ReadReg(336, &val); // PORT_CHSEL
TNU 0:dfe498e03679 870 if (res < 0)
TNU 0:dfe498e03679 871 goto END;
TNU 0:dfe498e03679 872 val >>= 16;
TNU 0:dfe498e03679 873 //pc->printf("chsel = %d\n", val);
TNU 0:dfe498e03679 874 cnt = 0;
TNU 0:dfe498e03679 875 // Count how many channels are selected
TNU 0:dfe498e03679 876 for (i = 0; i < 16; i++) {
TNU 0:dfe498e03679 877 if (val & 0x1)
TNU 0:dfe498e03679 878 cnt++;
TNU 0:dfe498e03679 879 val >>= 1;
TNU 0:dfe498e03679 880 }
TNU 0:dfe498e03679 881 nPack *= cnt;
TNU 0:dfe498e03679 882 nPack /= 16;
TNU 0:dfe498e03679 883
TNU 0:dfe498e03679 884 res = ReadReg(332, &val); // PORT_OVR_ACCUM_ACQ_OVR
TNU 0:dfe498e03679 885 // pc->printf("PORT_OVR = %d\n", (val >> 16));
TNU 0:dfe498e03679 886 val = (val >> 16) & 3; // Get bits 0 and 1
TNU 0:dfe498e03679 887
TNU 0:dfe498e03679 888 if (res < 0){
TNU 0:dfe498e03679 889 pc->printf("ReadReg Error1\n\r");
TNU 0:dfe498e03679 890 goto END;}
TNU 0:dfe498e03679 891
TNU 0:dfe498e03679 892 if (val == 0){ //00 = 1
TNU 0:dfe498e03679 893 nPack /= 4;
TNU 0:dfe498e03679 894 }
TNU 0:dfe498e03679 895 else if (val == 1){ //01 = 2
TNU 0:dfe498e03679 896 nPack /= 4;
TNU 0:dfe498e03679 897 }
TNU 0:dfe498e03679 898 else if (val == 2){ //10 = 4
TNU 0:dfe498e03679 899 nPack /= 2;
TNU 0:dfe498e03679 900 }
TNU 0:dfe498e03679 901 else if (val == 3){ //11 = 8
TNU 0:dfe498e03679 902 nPack /= 1;
TNU 0:dfe498e03679 903 }
TNU 0:dfe498e03679 904 else {
TNU 0:dfe498e03679 905 pc->printf("GetTrace: bad value\n");
TNU 0:dfe498e03679 906 }
TNU 0:dfe498e03679 907
TNU 0:dfe498e03679 908
TNU 0:dfe498e03679 909 // Write 1 to PORT_ACQU register and then wait
TNU 0:dfe498e03679 910 res = ReadReg(0x146, &val); // PORT_ACQU
TNU 0:dfe498e03679 911 if (res < 0){
TNU 0:dfe498e03679 912 pc->printf("ReadReg Error2\n\r");
TNU 0:dfe498e03679 913 goto END;}
TNU 0:dfe498e03679 914 val = (val>>16) | 1;
TNU 0:dfe498e03679 915 res = WriteReg(0x146, val); // PORT_ACQU
TNU 0:dfe498e03679 916 if (res < 0){
TNU 0:dfe498e03679 917 pc->printf("WriteReg Error3\n\r");
TNU 0:dfe498e03679 918 goto END;}
TNU 0:dfe498e03679 919
TNU 0:dfe498e03679 920 // Wait till PORT_READY bit is set.
TNU 0:dfe498e03679 921 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 922 cnt = 0;
TNU 0:dfe498e03679 923 while (((val & 0x10000) >> 16 != 1) && (cnt < 500)) {
TNU 0:dfe498e03679 924 wait_us(50);
TNU 0:dfe498e03679 925 res = ReadReg(470, &val); // PORT_READY
TNU 0:dfe498e03679 926 cnt++;
TNU 0:dfe498e03679 927 }
TNU 0:dfe498e03679 928
TNU 0:dfe498e03679 929 // Encode the request and send it
TNU 0:dfe498e03679 930 res = MLX_ReqReadTrc(tx);
TNU 0:dfe498e03679 931 if (res < 0)
TNU 0:dfe498e03679 932 goto END;
TNU 0:dfe498e03679 933
TNU 0:dfe498e03679 934
TNU 0:dfe498e03679 935 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 936 //----SHORT PACKET EXCHANGE--------
TNU 0:dfe498e03679 937 res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), 0);
TNU 0:dfe498e03679 938 if (res < 0)
TNU 0:dfe498e03679 939 goto END;
TNU 0:dfe498e03679 940
TNU 0:dfe498e03679 941 pc->printf("MOSI ReqReadTrace:\t 0x");
TNU 0:dfe498e03679 942 for(int i=0;i<(sizeof(tx));i++){
TNU 0:dfe498e03679 943 uint8_t* pnt=(uint8_t*)tx;
TNU 0:dfe498e03679 944 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 945 }
TNU 0:dfe498e03679 946 pc->printf("\n");
TNU 0:dfe498e03679 947 pc->printf("MISO ReqReadTrace:\t 0x");
TNU 0:dfe498e03679 948 for(int i=0;i<(sizeof(tx));i++){
TNU 0:dfe498e03679 949 uint8_t* pnt=(uint8_t*)rx;
TNU 0:dfe498e03679 950 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 951 }
TNU 0:dfe498e03679 952 pc->printf("\n");
TNU 0:dfe498e03679 953 pc->printf("-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n");
TNU 0:dfe498e03679 954 //----PACKET EXCHANGE
TNU 0:dfe498e03679 955 //--------------------------------------------------------------------------------------
TNU 0:dfe498e03679 956
TNU 0:dfe498e03679 957
TNU 0:dfe498e03679 958 // Optional status decoding
TNU 0:dfe498e03679 959 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 0:dfe498e03679 960 if (res < 0 || iserr)
TNU 0:dfe498e03679 961 goto END;
TNU 0:dfe498e03679 962
TNU 0:dfe498e03679 963
TNU 0:dfe498e03679 964 // Temporary pointer to uint16 data, for simplicity purpose
TNU 0:dfe498e03679 965 u16ptr = (uint16_t*)rxL.data;
TNU 0:dfe498e03679 966 //device.format(16,1);
TNU 0:dfe498e03679 967 trigger.write(1);
TNU 0:dfe498e03679 968 for (a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 969 {
TNU 0:dfe498e03679 970 res = MLX_EncodeStatusL1(&txL, 0, 0);
TNU 0:dfe498e03679 971 if (res < 0){
TNU 0:dfe498e03679 972 res = -7;
TNU 0:dfe498e03679 973 goto END;
TNU 0:dfe498e03679 974 }
TNU 0:dfe498e03679 975
TNU 0:dfe498e03679 976 //Tools::Wait(10);
TNU 0:dfe498e03679 977 wait_us(10);
TNU 0:dfe498e03679 978
TNU 0:dfe498e03679 979 // Clock the remaining long packets
TNU 0:dfe498e03679 980 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG1));
TNU 0:dfe498e03679 981 // 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 982 // 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 983 if (res < 0){
TNU 0:dfe498e03679 984 res = -8;
TNU 0:dfe498e03679 985 goto END;
TNU 0:dfe498e03679 986 }
TNU 0:dfe498e03679 987
TNU 0:dfe498e03679 988
TNU 0:dfe498e03679 989 // Decode the long responses, then extract data values
TNU 0:dfe498e03679 990 res = MLX_DecodeResL1(&rxL);
TNU 0:dfe498e03679 991 if ((res < 0)){
TNU 0:dfe498e03679 992 pc->printf("LONG READ ERROR: Stopped at the %d long message\n", a);
TNU 0:dfe498e03679 993 pc->printf("TXLONG MOSI Response: ");
TNU 0:dfe498e03679 994 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 995 uint8_t* pnt=(uint8_t*)(&txL);
TNU 0:dfe498e03679 996 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 997 }
TNU 0:dfe498e03679 998 pc->printf("\n\r");
TNU 0:dfe498e03679 999 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 1000 pc->printf("TXLONG MISO Response: ");
TNU 0:dfe498e03679 1001 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 1002 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 0:dfe498e03679 1003 pc->printf("%02X ", *(pnt+i));
TNU 0:dfe498e03679 1004
TNU 0:dfe498e03679 1005 }
TNU 0:dfe498e03679 1006 pc->printf("\n\r");
TNU 0:dfe498e03679 1007 pc->printf("-- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 1008 //printf("last RXL = \n");
TNU 0:dfe498e03679 1009 //for (i = 0; i < 300; i++) {
TNU 0:dfe498e03679 1010 // pc->printf("%02x", rxL.buf[i]);
TNU 0:dfe498e03679 1011 // if (((i + 1) % 8) == 0)
TNU 0:dfe498e03679 1012 // pc->printf("\n");
TNU 0:dfe498e03679 1013 //}
TNU 0:dfe498e03679 1014 //printf("\n");
TNU 0:dfe498e03679 1015 res = -9;
TNU 0:dfe498e03679 1016 goto END;
TNU 0:dfe498e03679 1017 }
TNU 0:dfe498e03679 1018
TNU 0:dfe498e03679 1019 // Copy the returned data into the user buffer
TNU 0:dfe498e03679 1020 // WTA: only half the MLX_LONG_DATA_SZ because we copy 2 bytes at a time
TNU 0:dfe498e03679 1021 for (b = 0; b < (MLX_LONG1_DATA_SZ / 2); ++b)
TNU 0:dfe498e03679 1022 {
TNU 0:dfe498e03679 1023 //WTA: removed MLX_HDR_SZ
TNU 0:dfe498e03679 1024 buf[a*(MLX_LONG1_DATA_SZ / 2) + b] = u16ptr[b];
TNU 0:dfe498e03679 1025 }
TNU 0:dfe498e03679 1026 if(a<64){
TNU 0:dfe498e03679 1027 pc->printf("Trace packet %d MOSI: \n\t\t\t\t\t0x", a);
TNU 0:dfe498e03679 1028 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 1029 uint8_t* pnt=(uint8_t*)(&txL);
TNU 0:dfe498e03679 1030 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 1031 if(((i %30) ==0)&&(i>0))pc->printf("\n\t\t\t\t\t");
TNU 0:dfe498e03679 1032 }
TNU 0:dfe498e03679 1033 pc->printf("\n\r");
TNU 0:dfe498e03679 1034 pc->printf("Trace packet %d MISO: \n\t\t\t\t\t0x", a);
TNU 0:dfe498e03679 1035 for(int i=0;i<(sizeof(PACK_LONG1));i++){
TNU 0:dfe498e03679 1036 uint8_t* pnt=(uint8_t*)(&rxL);
TNU 0:dfe498e03679 1037 pc->printf("%02X", *(pnt+i));
TNU 0:dfe498e03679 1038 if(((i %30) ==0)&&(i>0))pc->printf("\n\t\t\t\t\t");
TNU 0:dfe498e03679 1039 }
TNU 0:dfe498e03679 1040 pc->printf("\n\r");
TNU 0:dfe498e03679 1041 pc->printf("-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --\n\r");
TNU 0:dfe498e03679 1042 }
TNU 0:dfe498e03679 1043
TNU 0:dfe498e03679 1044 }
TNU 0:dfe498e03679 1045 trigger.write(0);
TNU 0:dfe498e03679 1046 device.format(8,1);
TNU 0:dfe498e03679 1047
TNU 0:dfe498e03679 1048 END:
TNU 0:dfe498e03679 1049 device.format(8,1);
TNU 0:dfe498e03679 1050 return res;
TNU 0:dfe498e03679 1051 }
TNU 0:dfe498e03679 1052
TNU 0:dfe498e03679 1053
TNU 0:dfe498e03679 1054 int LidarSpi::LoadPatch ( uint16_t address, uint8_t *buf, uint16_t nBytes){
TNU 0:dfe498e03679 1055 int res;
TNU 0:dfe498e03679 1056 uint16_t rSz;
TNU 0:dfe498e03679 1057 PACK_SHORT rx[1], tx[1];
TNU 0:dfe498e03679 1058 PACK_LONG2 rxL, txL;
TNU 0:dfe498e03679 1059 uint8_t iserr;
TNU 0:dfe498e03679 1060 uint16_t err;
TNU 0:dfe498e03679 1061 //uint32_t val = 0;
TNU 0:dfe498e03679 1062 uint16_t nWordsRemaining;
TNU 0:dfe498e03679 1063 uint16_t nPack;
TNU 0:dfe498e03679 1064 uint16_t nWords = nBytes / 2;
TNU 0:dfe498e03679 1065
TNU 0:dfe498e03679 1066
TNU 0:dfe498e03679 1067 // Ensure all packets are all zeros to not send trash
TNU 0:dfe498e03679 1068 memset(tx, 0, sizeof(tx));
TNU 0:dfe498e03679 1069 memset(&rxL, 0, sizeof(rxL));
TNU 0:dfe498e03679 1070
TNU 0:dfe498e03679 1071 //The Number of words to be transferred can be a multiple of LONG_DATA_SZ. Incase it is not
TNU 0:dfe498e03679 1072 //multiple of LONG_DATA_SZ, we do last packet with partial data and remaining words as 0
TNU 0:dfe498e03679 1073 nPack = nWords / (MLX_LONG2_DATA_SZ / 2) + (nWords % (MLX_LONG2_DATA_SZ / 2) == 0 ? 0 :1);
TNU 0:dfe498e03679 1074
TNU 0:dfe498e03679 1075 // Encode the request and send it
TNU 0:dfe498e03679 1076 res = MLX_ReqWriteFW(tx, nPack, address);
TNU 0:dfe498e03679 1077 if (res < 0)
TNU 0:dfe498e03679 1078 goto END;
TNU 0:dfe498e03679 1079
TNU 0:dfe498e03679 1080 res = TxPacketSlow((uint8_t*)rx, &rSz, (uint8_t*)tx, sizeof(tx), 0);
TNU 0:dfe498e03679 1081 if (res < 0)
TNU 0:dfe498e03679 1082 goto END;
TNU 0:dfe498e03679 1083
TNU 0:dfe498e03679 1084 // Optional status decoding
TNU 0:dfe498e03679 1085 res = MLX_DecodeStatusS(rx, &iserr, &err);
TNU 0:dfe498e03679 1086 if (res < 0 || iserr)
TNU 0:dfe498e03679 1087 goto END;
TNU 0:dfe498e03679 1088
TNU 0:dfe498e03679 1089 nWordsRemaining = nWords;
TNU 0:dfe498e03679 1090 for (uint a = 0; a < nPack; ++a)
TNU 0:dfe498e03679 1091 {
TNU 0:dfe498e03679 1092 uint size;
TNU 0:dfe498e03679 1093 if (nWordsRemaining > (MLX_LONG2_DATA_SZ / 2))
TNU 0:dfe498e03679 1094 size = MLX_LONG2_DATA_SZ / 2;
TNU 0:dfe498e03679 1095 else
TNU 0:dfe498e03679 1096 size = nWordsRemaining;
TNU 0:dfe498e03679 1097
TNU 0:dfe498e03679 1098 res = MLX_WriteDataL2(&txL, size, a, &buf[a*MLX_LONG2_DATA_SZ]);
TNU 0:dfe498e03679 1099 if (res < 0){
TNU 0:dfe498e03679 1100 res = -7;
TNU 0:dfe498e03679 1101 goto END;
TNU 0:dfe498e03679 1102 }
TNU 0:dfe498e03679 1103
TNU 0:dfe498e03679 1104 //Tools::Wait(10);
TNU 0:dfe498e03679 1105
TNU 0:dfe498e03679 1106 // Clock the remaining long packets
TNU 0:dfe498e03679 1107 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 1108
TNU 0:dfe498e03679 1109 // Decode the long responses
TNU 0:dfe498e03679 1110 res = MLX_DecodeResL2(&rxL);
TNU 0:dfe498e03679 1111 if (res < 0){
TNU 0:dfe498e03679 1112 printf("LONG WRITE ERROR: Stopped at the %d long message\n", a);
TNU 0:dfe498e03679 1113 res = -9;
TNU 0:dfe498e03679 1114 goto END;
TNU 0:dfe498e03679 1115 }
TNU 0:dfe498e03679 1116
TNU 0:dfe498e03679 1117 nWordsRemaining = nWords - size;
TNU 0:dfe498e03679 1118 }
TNU 0:dfe498e03679 1119
TNU 0:dfe498e03679 1120 //LAST STATUS LONG PACKET FROM 75320 to get status of last write long
TNU 0:dfe498e03679 1121 res = MLX_EncodeStatusL2(&txL, 0, 0);
TNU 0:dfe498e03679 1122 if (res < 0) {
TNU 0:dfe498e03679 1123 res = -7;
TNU 0:dfe498e03679 1124 goto END;
TNU 0:dfe498e03679 1125 }
TNU 0:dfe498e03679 1126
TNU 0:dfe498e03679 1127 //Tools::Wait(10);
TNU 0:dfe498e03679 1128 // Clock the remaining long packets
TNU 0:dfe498e03679 1129 res = TxPacket((uint8_t*)&rxL, &rSz, (uint8_t*)&txL, sizeof(PACK_LONG2));
TNU 0:dfe498e03679 1130
TNU 0:dfe498e03679 1131 if (res < 0)
TNU 0:dfe498e03679 1132 goto END;
TNU 0:dfe498e03679 1133
TNU 0:dfe498e03679 1134 // Change jump table pointer.
TNU 0:dfe498e03679 1135 res = WriteReg(0x1000, 0x7000); // write addr: 0x1000 value:0x7000
TNU 0:dfe498e03679 1136 if (res < 0)
TNU 0:dfe498e03679 1137 goto END;
TNU 0:dfe498e03679 1138
TNU 0:dfe498e03679 1139 END:
TNU 0:dfe498e03679 1140 return res;
TNU 0:dfe498e03679 1141 }
TNU 0:dfe498e03679 1142
TNU 0:dfe498e03679 1143 void LidarSpi::Trigger(int level){
TNU 0:dfe498e03679 1144 trigger.write(level);
TNU 0:dfe498e03679 1145 }