femtoduino / ArduinoCore-atsamd21e18a

Arduino Core and Bootloader for the ATMEL SAM D21/R21 (E Variant) chips.
https://femto.io
29 stars 15 forks source link

SET_REPEAT:0x01::D command not working #20

Closed davidvondle closed 4 years ago

davidvondle commented 6 years ago

I have programmed the dongle and coins with the most recent code. If open a terminal to get a datastream from one of the coins using: :1 :START_RTC SET_REPEAT:0x01::D it runs for <1 second then stops. I can send another command to get it to run again, but this time the yaw,roll,and 3 euler angles respond with NaN.

zrecore commented 6 years ago

Reproduced the issue. I'll have a look on Monday. Thank you for filling the issue here.

drogge commented 6 years ago

Took a look at the FemtoCore source and to be honest couldn't figure out how repeat commands are repeated. Unless it's done by sending the command over the network again. One thing that looks kind of sketchy is the use of static buffers in a lot of the functions. If any of the code gets reentered the buffers could overflow. This may eventually lead to _data_flow_command overflowing into kFilters or something. No hardware to test things on. Good luck.

zrecore commented 6 years ago

Took a look at the FemtoCore source and to be honest couldn't figure out how repeat commands are repeated. Unless it's done by sending the command over the network again. One thing that looks kind of sketchy is the use of static buffers in a lot of the functions. If any of the code gets reentered the buffers could overflow. This may eventually lead to _data_flow_command overflowing into kFilters or something. No hardware to test things on. Good luck.

The "SET_REPEAT" command runs on the dongle, and re-issues the supplied command once a response is received.

:1
:START_RTC
SET_REPEAT:0x01::D

@drogge ping me via e-mail or twitter (support@femtoduino.com), I can get you some test hardware.

drogge commented 6 years ago

I'm having a bit recreating this bug. However I will admit that I have recompiled the source obtained from download.femtoduino.com. The closest I've come to it is having the dongle quit reporting results from the coin D command. One this of note is that the led on the coin is red. From the FemtoCore.cpp source this means that NWK_DataReq yielded a NWK_PHY_NO_ACK_STATUS status. The coin starts reporting correct results if the SET_REPEAT:0x01::D command is re-issued on the dongle.

I think that the NaN's reported on the original bug report is because the IMU got reset somehow. Either that or the coin reset it self.

drogge commented 6 years ago

Looking further, it seems there's a time element involved with FreeIMU. For example if, through the Arduino serial monitor, I init the IMU with the '1' command and immediately display the IMU results with the 'D' command I get reasonable results:

1:OK298908,-2.9625,0.0255,0.1797,359.5229,1.4689,349.7203,-1.1646,1.1419,1.2877 300122,-2.9067,0.0036,0.1924,336.7409,0.1701,348.8600,-0.3685,0.3536,0.7139 300919,-2.5070,-0.0510,0.1880,294.7010,349.3136,350.3329,0.5123,-0.5187,0.0764 301634,-2.4459,-0.1832,0.1798,257.0276,348.0993,350.6876,1.2414,-1.2408,-0.4516

Even if I init the IMU and wait 10 seconds I still get a good reading:

1:OK857198,-2.6292,0.0000,0.0000,359.1136,0.0000,-0.0000,1.4589,-1.4561,-0.6125

However if I wait another 10 seconds I get an invalid reading with NaN's:

866219,-2.5751,nan,nan,nan,nan,nan,0.5479,-0.5540,0.0426

I now think that what's happening in the original bug report is that some network error causes the coin to either not send a response to the D command or the dongle to miss the response from the coin. Then if you wait too long before restarting the SET_REPEAT command the coin's IMU will time out and start yielding NaNs.

The time sensitive code in the FreeIMU code seems to be these lines in function FreeIMU::getQ(float q, float val) in FreeIMU.cpp:

now = micros(); sampleFreq = 1.0 / ((now - lastUpdate) / 1000000.0); lastUpdate = now;

zrecore commented 6 years ago

Looking further, it seems there's a time element involved with FreeIMU. For example if, through the Arduino serial monitor, I init the IMU with the '1' command and immediately display the IMU results with the 'D' command I get reasonable results:

1:OK298908,-2.9625,0.0255,0.1797,359.5229,1.4689,349.7203,-1.1646,1.1419,1.2877 300122,-2.9067,0.0036,0.1924,336.7409,0.1701,348.8600,-0.3685,0.3536,0.7139 300919,-2.5070,-0.0510,0.1880,294.7010,349.3136,350.3329,0.5123,-0.5187,0.0764 301634,-2.4459,-0.1832,0.1798,257.0276,348.0993,350.6876,1.2414,-1.2408,-0.4516

Even if I init the IMU and wait 10 seconds I still get a good reading:

1:OK857198,-2.6292,0.0000,0.0000,359.1136,0.0000,-0.0000,1.4589,-1.4561,-0.6125

However if I wait another 10 seconds I get an invalid reading with NaN's:

866219,-2.5751,nan,nan,nan,nan,nan,0.5479,-0.5540,0.0426

I now think that what's happening in the original bug report is that some network error causes the coin to either not send a response to the D command or the dongle to miss the response from the coin. Then if you wait too long before restarting the SET_REPEAT command the coin's IMU will time out and start yielding NaNs.

The time sensitive code in the FreeIMU code seems to be these lines in function FreeIMU::getQ(float q, float val) in FreeIMU.cpp:

now = micros(); sampleFreq = 1.0 / ((now - lastUpdate) / 1000000.0); lastUpdate = now;

Ok, that sounds like it makes sense. Perhaps I can keep calling a FreeIMU function (if enabled) by adding FemtoCore::handleIMU(), which can then be called in the sketch loop() method.

garnould commented 5 years ago

Last commit in master (merged yesterday) seems to fix this bug (SET_REPEAT). Can you test and send feedback ?

garnould commented 5 years ago

Hi there,

I tried to fix NaN bug while traveling by train with no HW to test fix.

Commit is here => https://github.com/garnould/ArduinoCore-atsamd21e18a/tree/paneer_nan (branch contains only 1 commit ahead of master).

Didn't pull request because not tested yet. It compiles properly and should do the job.

Mainly : rewrote FemtoCore::sendSampleLegacy()

Until someone has tested until next monday, I'll send feed back then when I'll have hands on HW.

G.

garnould commented 5 years ago

Saw NaN's is some (not yet repeated) conditions : bug is not yet completely fixed.

drogge commented 5 years ago

Garnould, have you read my comment dated Oct. 1st? I'm almost positive that the NaN's are coming from the FreeIMU library. It seems to have something to do with doing a bunch of reads from the IMU, then waiting for something like 10 seconds and then reading again. I think the original bug report is the result of the running the SET_REPEAT command, the Beacon missing response from the coin and then restarting the SET_REPEAT without reinitializing the coin's IMU. This fits the "read a bunch, wait awhile and then read some more" pattern mentioned above. In converting the IMU hardware reading to something reasonable there seems to be some kind of a timing issue.I think it has to do with reading the gyroscope. I've been working with the FreeIMU lib separate from FemtoCore and to be honest I haven't had much luck; inconsistent behavior with running the same code twice, etc. It's all pretty much magic to me right now.

garnould commented 5 years ago

Hi drogge,

Thank you for pointing this to me.

I managed to reproduce the bug with coin:

I'll have a look at it.

garnould commented 5 years ago

Located the beginning of the bug:

'q' being NaN's after getQ, NaN's received at the end.

In getQ, it looks like q (param) should be receive values from q0, q1, q2 and q3 (from FreeIMU class).

Except at init time, q0..q3 can be fed with values from:

Meaning at the moment, q0..q3 are receiving values only at init time.

When removing comment syntax on getQ_simple call, no more NaN's but the returned values of YPR are quite strange.

@Alex: any magnetometer in FemtoBeacon ?

zrecore commented 5 years ago

@garnould yes, it has a built in magnetometer. The IC is the MPU-9250

zrecore commented 5 years ago

Note, send :1, then :2, then :START_RTC

drogge commented 5 years ago

I've traced the problem down to bogus things happening to the values in the DCM_Matrix in ../DCM/DCM.cpp. Bogus is defined as values overflowing and/or being set to 0. Either of these will cause NaNs. The problem seems to be accumulative; some values will get larger with each call into the DCM stuff and eventually overflow. The value from the micros() call in FreeIMU::getQ() seems to be pretty sensitive.

One thing to note is that I've never gotten a NaN when playing with the example sketches FreeIMU github distribution.

Drew

garnould commented 5 years ago

@zrecore : does this repo FreeIMU code come from https://github.com/mjs513/FreeIMU-Updates/ ?

zrecore commented 5 years ago

@garnould yes. Unfortunately, I had to include it here as part of this package due to the FreeIMU folder structure. It's the only way I could get it into the board package.

drogge commented 5 years ago

I was wondering why it was included in the package. That is, until I installed it in my normal Arduino library directory. It's going to be a drag to clean it out if I ever have to

drogge commented 5 years ago

Alex, Why is it necessary to issue the START_RTC command when using the IMU?

zrecore commented 5 years ago

It starts the RTC timer, which is needed by the modded RTC Zero library. Issuing the "D" command returns the timestamp ticks counted since initialization.

On Wed, Nov 14, 2018, 8:28 PM Drew Rogge <notifications@github.com wrote:

Alex, Why is it necessary to issue the START_RTC command when using the IMU?

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/femtoduino/ArduinoCore-atsamd21e18a/issues/20#issuecomment-438913385, or mute the thread https://github.com/notifications/unsubscribe-auth/AAaLbF5pFIwc3_p_sVA4b9Do_kk9npVKks5uvO1jgaJpZM4Wyuvq .

zrecore commented 5 years ago

Note, sleep/wake also depends on the RTC Zero library

garnould commented 5 years ago

I was wondering why it was included in the package. That is, until I installed it in my normal Arduino library directory. It's going to be a drag to clean it out if I ever have to

I had a look at how to remove it from package and have it beside, as a modified fork of https://github.com/mjs513/FreeIMU-Updates/ (Alex patched it). This would let us rebase our fork ~from upstream branch.

arduino-builder supports several libraries paths but it looks that Arduino IDE doesn't. It'll require some patch (then pull request and merge by arduino/arduino to be supported).

zrecore commented 5 years ago

@garnould to keep this work within scope, perhaps having the FreeIMU libraries reworked into a patch should be assigned as a different GitHub issue.

garnould commented 5 years ago

@zrecore ok with you. Then this issue can be closed (normaly, SET_REPEAT keeps on working now until disables).

As pointed by Drew, NaN problem hides behind those lines

`void FreeIMU::getQ(float q, float val) { // stuff deleted

if (MARG == 4 && IS_9DOM() && not defined(DISABLE_MAGN))

val[9] = maghead.iheading(1, 0, 0, val[0], val[1], val[2], val[6], val[7], val[8]);
    dcm.setSensorVals(val);
    dcm.G_Dt = 1./ sampleFreq;
    dcm.calDCM();
    dcm.getDCM2Q(q);

endif

// stuff deleted }`

"FreeIMU package" included in Femtoduino has quite heavily forked from the upstream. @zrecore would you reinstall the original file and re-insert your modifications before trying to handle several libs folders in IDE (other issue/subject) ?

garnould commented 5 years ago

Update on this:

garnould commented 5 years ago

Small update:

At this point, I need help : Alex, an you trying to do the same and get a "vanilla" (then patched) repo of FreeIMU working outside FemtoCore repo ?

G.

zrecore commented 5 years ago

There's a bunch of Serial.print calls in FreeIMU Utils and a few other places which I patched so Serial is actually SerialUSB...

I had to define the board in FreeIMU.h as MPU9250_5611

I had also made a kludgy patch in FreeIMU.h at some point to avoid having "calibration.h" included if it had already been included (so I could include a calibration header next to my sketch instead of within the FreeIMU library).

On Mon, Nov 26, 2018, 1:00 PM Georges A <notifications@github.com wrote:

Small update:

  • succeeded in removing FreeIMU stuff from FemtoCore and compile against a ~clone of https://github.com/mjs513/FreeIMU-Updates/ using patched IDE (arduino/Arduino#8221 https://github.com/arduino/Arduino/pull/8221)
  • had to reproduce Alex' changed made in FreeIMU.cpp/FreeIMU.h. I tried to not miss any but am not sure I didn't ...
  • compiled firmware get stuck after '1' command in freeIMU.init(true); : Serial.println("FreeIMU started."); never gets displayed.

At this point, I need help : Alex, an you trying to do the same and get a "vanilla" (then patched) repo of FreeIMU working outside FemtoCore repo ?

G.

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/femtoduino/ArduinoCore-atsamd21e18a/issues/20#issuecomment-441796163, or mute the thread https://github.com/notifications/unsubscribe-auth/AAaLbGBISpXuZtdlixAeYbyHqNUinTD8ks5uzFZugaJpZM4Wyuvq .

drogge commented 5 years ago

I'm also looking into using the latest FreeIMU but still including it in Femtoduino core. One thing I've found is that the call to accgyro.initialize9250MasterMode(); in FreeIMU caused init() to not return.

I also found that having FreeIMU installed in the sketchbook Arduino/libraries causes the Arduino IDE to ignore the FreeIMU included with the femto core.

Just a word of warning but be careful with what you're doing. I've managed to somehow create code that will cause both the beacon and coin to not be recognized by my Mac. Alex told me how to reset back into the boot loaded but I haven't tried it yet.

garnould commented 5 years ago

@drogge

I had to (hard) reset my coin using reset pads, as explained by Alex. Once done, I could re-upload sketches to my coin (as normally) but it kept changing of port name under OSX at every reset. Very annoying. Hence the patch and pull request I made (not merged yet) than append a serial number to the USB description (computed from chip serial number). It work perfectly on OSX.

I just managed to have the latest/official FreeIMU lib ~working : accgyro.initialize9250MasterMode(); now returns on my setup. I renamed all Serial. to SerialUSB. in MPU60X0.cpp and MPU60X0.h files and enabled DEBUG.

I still have "NaN" values as shown below.


FemtoCore::_setupSerial() complete. FemtoCore::init() > Serial complete. This Node Address: 0x2 Dest Node Address: 0x1 Endpoint: 0x1 Broadcast PAN ID: 0x1 Channel: 0x1A TX Power: 0x0 Antenna: 0x1 FemtoCore::updateNetworkingConfig() complete. FemtoCore::_setupMeshNetworking() complete. FemtoCore::_setupSensors() initializing... FemtoCore::_setupSensors() complete. FemtoCore::send() non-chunked, max buffer size is 105. Processing =INIT_COMPLETE (size 14). FemtoCore::send() non-chunked, data is: =INIT_COMPLETE FemtoCore::_networkingSendMessage() sending message =INIT_COMPLETE (size 14). FemtoCore::_networkingSendMessage() complete. FemtoCore::send() from 0x2 to 0xFFFF on endpoint 0x1 complete. FemtoCore::init() complete. FemtoCore::_networkingSendMessageConfirm() processing... FemtoCore::_networkingSendMessageConfirm() NWK_SUCCESS_STATUS FemtoCore::_networkingSendMessageConfirm() complete. GLOBAL serialEvent() 1 FemtoCore::handleSerialRx() inputString data (1) length is: 1 FemtoCore::handleSerialRx() input_buffer data (1) length is: 1 FemtoCore::handleSerialRx() first char is 1 FemtoCore::handleSerialRx() buffer at +1 is FemtoCore::handleSerialRx() processing command (1) locally FemtoCore::processCommand got (1), length :1 FemtoCore::processCommand is_femtobeacon_coin : 1 FemtoCore::processCommand checking for only 'is_femtobeacon_coin' commands Starting FreeIMU (fastmode disabled). AK Not Powered Down FUSE ROM Access Not set 177.00, 131.00, 39.00 Mag Finished: 45846 65 0 FreeIMU started. 1:OKFemtoCore::processCommand checking for common commands GLOBAL serialEvent() D FemtoCore::handleSerialRx() inputString data (D) length is: 1 FemtoCore::handleSerialRx() input_buffer data (D) length is: 1 FemtoCore::handleSerialRx() first char is D FemtoCore::handleSerialRx() buffer at +1 is FemtoCore::handleSerialRx() processing command (D) locally FemtoCore::processCommand got (D), length :D FemtoCore::processCommand is_femtobeacon_coin : 1 FemtoCore::processCommand checking for only 'is_femtobeacon_coin' commands FemtoCore::sendSampleLegacy() replying with '20990,-2.3394,0.0000,0.0000,359.5637,0.0000,-0.0000,0.0393,-0.0532,-0.0152'

20990,-2.3394,0.0000,0.0000,359.5637,0.0000,-0.0000,0.0393,-0.0532,-0.0152 FemtoCore::processCommand checking for common commands GLOBAL serialEvent() D FemtoCore::handleSerialRx() inputString data (D) length is: 1 FemtoCore::handleSerialRx() input_buffer data (D) length is: 1 FemtoCore::handleSerialRx() first char is D FemtoCore::handleSerialRx() buffer at +1 is FemtoCore::handleSerialRx() processing command (D) locally FemtoCore::processCommand got (D), length :D FemtoCore::processCommand is_femtobeacon_coin : 1 FemtoCore::processCommand checking for only 'is_femtobeacon_coin' commands FemtoCore::sendSampleLegacy() replying with '23360,-2.9189,nan,nan,nan,nan,nan,0.0457,-0.0643,-0.0434'

23360,-2.9189,nan,nan,nan,nan,nan,0.0457,-0.0643,-0.0434 FemtoCore::processCommand checking for common commands GLOBAL serialEvent() D FemtoCore::handleSerialRx() inputString data (D) length is: 1 FemtoCore::handleSerialRx() input_buffer data (D) length is: 1 FemtoCore::handleSerialRx() first char is D FemtoCore::handleSerialRx() buffer at +1 is FemtoCore::handleSerialRx() processing command (D) locally FemtoCore::processCommand got (D), length :D FemtoCore::processCommand is_femtobeacon_coin : 1 FemtoCore::processCommand checking for only 'is_femtobeacon_coin' commands FemtoCore::sendSampleLegacy() replying with '30175,2.7901,nan,nan,nan,nan,nan,0.0519,-0.0749,-0.0705'

30175,2.7901,nan,nan,nan,nan,nan,0.0519,-0.0749,-0.0705 FemtoCore::processCommand checking for common commands

drogge commented 5 years ago

What did you have to do to make initialize9250MasterMode() work? TI don't know what what they actually mean but the lines: AK Not Powered Down and FUSE ROM Access Not set make me think things aren't working correctly with the IMU.

To be honest I find the whole IMU thing to be real frustrating. I've tried multiple software suites, different MPU9250 board and different CPU boards and I can't get anything to work correctly and repeatably. Also the different code suites seem contradictory. For example some say the IMU should return 0x71 from the WHO_AM_I register and some say 0x68. On the femto coin I get 0x38. With a different board on a Feather M0 I get 0x68.

As far as I'm concerned, it's all magic right now. I'd love to find a setup that "just works."

garnould commented 5 years ago

Hi Drew,

void MPU60X0::initialize9250MasterMode makes very strange/unsafe registers manipulations as it does handle them as "one register for one use" and this is not the case: registers can have several purposes based on bit ranges in the register.

In the current context, I am event not sure that it :

I'll have a look at it.

BTW, I do fully agree with you : FreeIMU behavior is a bit messy.

zrecore commented 4 years ago

@davidvondle Latest fixes in commit 7bea39e (which is now version 1.8.6) should handle this issue. Would you mind trying out the changes? (See example FemtoCore sketch, which adds FemtoCore::handleIMU() call to the loop() method)