Code Monkey home page Code Monkey logo

rmr_sensor_fusion's People

Contributors

im-kitsch avatar malbreitenbach avatar xianglunchen avatar

Watchers

 avatar

rmr_sensor_fusion's Issues

The small bug

@malbreitenbach
Hi, I've been pushed the .gitignore of the truestudio project and you can also update your SPI code. Filter is not very strict because some files like .project are also tracked.
Furthermore, if you have time, can you have a try about my code in my branch? It's just a small test about LED Blinking with "i--" format delay. I can toggle the LED with button but the loop will not be excuted normally. The value shows "optimized out". Both for loop or while loop seems wired, only sometimes I make the single step debug, LED will blink. If you have time you can also take a look at it. I'm a little confused of it.
Cheers.

Protocol improvements

Hello @raultron, hello @dino,
As you saw in #10 we need to increase the protocol's performance. One way is to try the DMA on the Nucleo, but to be honest I don't have any experience in DMA. I'm also not sure if this would increase the performance by far, because the measured performance (330Hz) is not limited by the Nucleo but mostly by the HLP (which does not support DMA) at this point.

I was thinking about two different protocol implementations instead:

1st Option

HLP Nucleo
Read Sensory Wait for Handshake
Performing Handshake Performing Handshake
Send Sensory data Receive Sensory data
Wait for Handshake Perform SensorFusion / Control algorithm
Performing Handshake Performing Handshake
Receive Control data Send Control data
Use Control data Wait for Handshake
Read Sensory Wait for Handshake
[...] [...]

2nd Option

HLP Nucleo
[...] [...]
Performing Handshake Performing Handshake
Send new Sensory data / Receive last Control data Send Control data/Receive Sensory data
Read Sensory / Use Control data Perform SensorFusion / Control algorithm
Performing Handshake Performing Handshake
Send new Sensory data / Receive last Control data Send Control data/Receive Sensory data
Read Sensory / Use Control data Perform SensorFusion / Control algorithm
[...] [...]

While the 1st option is not using the duplex functionality, the second option is. There is a classic tradeoff between throughput and latency. The first option has the minimal latency, because the Nucleo is waiting for the sensory data, then performing the SensorFusion immediately and is sending back the calculated control signals right after that.
On the other hand the second option allows a higher throughput, because while the HLP is already reading the new sensory data (from the sensors) the Nucleo is performing the SensorFusion on the "old" sensory data. But if the "Sensor Fusion"-duration is shorter than the "Sensory Reading"-duration the latency will be increased.

Edit: I'm not sure, if the tables are showing the idea clearly enough. If not, I will add some graphics.

Paper

Hello @raultron and @dinu,
how should we refer to our written code? Using a github-link or better adding it to the attachments?

Thanks for reply in advance!

Code profiling

The last days we have tried to figure out what parts of our code are time expensive. We followed several
approaches, but not with 100% success, yet. Best approach was using a timer and compare the counter at certain places in the code. We are able to store the current counter value inside a variable, but we are not able to read this variable while debugging (Watch expression isn't working reliably ). So we tried to read the counter value directly out of the memory (see below)

timer1

But it seems like 'pause at breakpoint' -> 'read the value' -> 'run' -> 'pause at next breakpoint' -> [...] is not accurate enough, because the 'run & pause' also seems to need some time.

So we need to find a solution to store the current counter inside a variable.
We already tried:

volatile unsigned long ticks = T1TC; //Reading '0xffffffff' at any time
volatile unsigned int ticks = T1TC; //Reading '0xffffffff' at any time
volatile uint32_t ticks = T1TC; //Reading '0xffffffff' at any time

unsigned long ticks = T1TC; 
volatile unsided long avoidOptimization = ticks; 
/* Works sometimes if you set the breakpoint right afer the declaratoin */

Unfortunatly we haven't found a solution that is working in any time, so maybe someone has an idea about that? @raultron @im-Kitsch @dinu

Sensorfudions

Updatae rate, parallel or not?
Iteration rule.
........ to update

Unwanted union behavior (incl. ** Workaround **)

Hello @raultron, hello @dino,
as Dino pointed out while last meeting we have some problems using the following union:

typedef union {
	struct {
		/***	--- Protocol head --- ***/
		uint8_t startByte; //Status Flags; 0: Valid, 1: ...

		/*** 	--- Sensory Data --- ***/
                int angle_pitch;
		int angle_roll;
		int angle_yaw;

                 /*** --- Protocol Footer --- ***/
		uint8_t checksum;
	} protocol_s;

	uint8_t bytestream[num_sum];
}protocol_u;

While using this union the following irregularity occurs:
union_error

As you can see in the picture the union is not working as expected. For the first four variables you can see the correct alignment between the struct protocol_s and the bytearray bytestream. But when you compare the last byte in the union (checksum = 7), you can see that bytestream[13] (= 0) is not consistent anymore.
As Dino already mentioned, this problem is called Padding. Padding baiscally is called the way the C compiler lays out basic C datatypes in memory in order to make memory accesses faster (see detailed explanation). In our specific case, the compiler seems to add some Zero-Bytes after angle_yaw in order to place checksum at an aligned position for faster access. To avoid that compiler optimization we tried to use

#pragma pack(push,1) 
typedef union {
[...]
}protocol_u;
#pragma pack(pop)

That is working quite good for the Nucleo, no padding anymore so the alignment is like expected. The only thing that should be mentioned is the swap between MSB and LSB while using the #pragma. But this is easily corrected by a simple for - loop, which is swapping the Bytes.

Unfortunately for the HLP the #pragma isn't working with error message:

protocol.h:19: warning: #pragma pack(push[, id], ) is not supported on this target
protocol.h:51: warning: #pragma pack(pop[, id], ) is not supported on this target

I haven't found a solution, yet.

** Workaround **
We found out by testing that padding is only used when a greater variable is followed by a smaller variable, so for example

		uint8_t var1; //8Bit
                int var2; //32Bit
		int var3; //32Bit
		int var4; //32Bit
                uint8_t var5; //8Bit (PADDING)

uses padding, while

		uint8_t var1; //8Bit
                uint16_t var2 //16Bit
                uint16_t var3 //16Bit
                int var4; //32Bit
		int var5; //32Bit
		int var6; //32Bit

uses no padding at all and is working for us. Of cause we are not 100% sure if that works in any situation, but it works in 5 out of 5 tested ones. We are using that method while we don't know how to fix that error. Anyway, the limitation is not that big, you just have to be aware about the right order of your variables. To see our current working solution, visit our Code.

Many thanks in advance for your time and help!

Mechanical mount

We started to design a mount for the Nucleo (See "data/mount"). We haven't decided where the mount should be placed onto the firefly. We basically thought about two variants with the following pros/cons:

Mounting on top Mounting to the side
+ maintain the balance - unbalance
- Covering the GPS-/magnetic sensor (possibly additional metal plate for sheelding nescessery) + most likely no additional shielding nescessery

Problem now about PX4FLOW

Using I2C in HLP to read the data from PX4FLOW. But the driver doesn't work. It is really strangly achieved. And there is no function like I2C transmit or I2CReceive. Only one sample. It's sending data to motor.

void I2C0_send_motordata(void)
{
	WrIndex=0;
	RdIndex=0;
	I2CWriteLength = 5;
	I2CReadLength = 0;
    I2CMasterBuffer[0] = 0x02;
    I2CMasterBuffer[1] = 100;
    I2CMasterBuffer[2] = 100;
    I2CMasterBuffer[3] = 100;
    I2CMasterBuffer[4] = 1;

    //I20CONSET = I2CONSET_STA;	/* Set Start flag */
    //if ( !I2CStart() ) I2CStop();
    I2CCmd = GET_TEMPERATURE;
	I2CEngine();
}

Seems to use Interrupt with buffer to tranmitting data. In communication with TongXun, she say it doesn't work. Will try to make a new driver.

Sorting Git Repository

@raultron
we want to sort out our github resposity. Because the current version always puzzles us and some file-tracking is very orderless. We fixed many times but it doesn't work well. It wastes too much time to migrate the code.
Now we decide to sort out our project make it systematic. We plan to firstly clear the current resposity, Then divide the project to followed branches. The current status of resposity is really confused. And we haven't efficiently used git these days.

--> Branch-Nucleo-Master
|--> Important Branch Nucleo 1
|--> Important Branch Nucleo 2
..........
--> Branch-HLP-Master
|--> Important Branch HLP 1
|--> ...........
--> Branch-Data.

Our project will be based on these three master branches. (When we make some staged progress we will create a new branch to save it, e.g. when we achiveve some partial functions like SPI, IIC.)
So that we can indenpently tracking the progress of different boards' code. And we just need one branch to wirte data. So we also create a new branch as data indenpendtly to save documents.

But the struct will be quite different to what you expected in the 1.st Issue. Maybe we should firstly ask your permission. It's not standard to manage the project but it's proper for us now. We still focus on main branch but just like divide master to three master branch.

We have created the described branches and you can check it. If okay we will delete other useless branches and reserve a backup branch.

Reorganization of the folders

I think a proper folder structure should be comprised of two folders for the moment. One folder named "data" and another folder called "src" (here goes the C/C++ source code of your project), don't include compiled binaries on the "src" folder.

Inside "data" you can upload later the final pdf of the report and the final slides, you can also have a folder inside "data" named "literature" were you can put all the relevant papers. Place inside "data" all the logs and documents that you want to create for the project in proper folders. For example, you should move the files "Basic Notes.md" and "WorkPlan.md" to a folder inside "data".

The README belongs to the root folder as it is right now.

So the repo should look:

/data
/src
README.md

Remember that all the code, comments and documentation should be in English and take a Git tutorial so you can properly use the Code Versioning tools instead of using this like a Dropbox folder.

Cheers!

Meeting with Tong Xun

  1. The operating system of Mastermind doesn't work now. It seems like that it's not suitable for Ubuntu 16.04 and they reinstall it as Ubuntu 12.
  2. The program of HL Processor is not original version. We need to update it.
  3. We can also just use the Mastermind in UAV

More things to be clarified in Friday.

Some notes about flashing HL and plug

Flashing program to HL in win10 still doesn't work. We give up to solve selfly and wait the reply of customer service.

For the using of Ubuntu to download program, I will upload later in github.

The plug we tried to soldering and only crimping. And it actually need to be crimped rather than solder cause the socket is too small. Not only for us but also for the UAV fans in the world a nightmare. So we give up make it manually and search the socket already assembly with cable.
The link are here:
This is the link in amazon, it's the already finished connector. We can just destroy the plastic shell and use the cabel. Just because the shipping is better.

This is recommended, but shipping not guranteed.
This is also good, but it need to be read carefully.

The problmen 5.4:

The driver in the website doesn't work. The PC can not recognize the device.
One of the motor seems to work not regularily.

Accuracy of the timestamp (Low Priority Issue)

We added a timestamp to our protocol message for testing reasons. This timestamp shows when the message was created. Therefore it is an indicator how much time it needs for one message to

  1. build the message (later: sensory data)
  2. sent to the Nucleo
  3. process on the nucleo (build a reply message depending on the received data) / build reply message (later: control data)
  4. sent reply message back to the HLP
  5. reconstruct reply message

For reference a timer value is used. The Timer is of cause configurable, so we can decide about the accurancy. At the moment we are transmitting messages at 500 - 1000Hz (meaning there is one message every 1ms - 2ms).
@raultron @dinu Any assessments about a valid accurancy?

  • Higher Accurancy (faster count) -> More Overflows / bigger counter range needed (more Bytes to transmit per message)

  • Lower Accurancy (slower count) -> Less Overflows / smaller counter range needed (less Bytes to transmit per message)

Interrupt based transmitting (performance limit)

Hey @raultron @im-Kitsch @dinu,
Today we tested the performance of the interrupt based (duplex) transmitting on the Nucleo (Slave). It's working perfectly for low transfer speeds like 250kHz. But when we increase the speed to our proven 4MHz it shows some irregularities.

Transmitted Bytestream Received Bytestream
1 1
2 2
3 3
10 10
11 1
12 1
21 1
22 11
12
3
3
21
22

So the correct Bytestream is basically received, but there are some older values pushed in. I did a testing what happens when you don't react to a FIFOTXbufferIsEmpty interrupt after a certain time. It turns out that it is sending one older value in that case (3 to 5 values before). I think the Nucleo cannot keep up to the HLP's transfer speed (4MHz). The Interrupt load seems to be to high to react just in time to the FIFOTXbufferIsEmpty interrupt. So I will try to push the clocks a little more and have a try tomorrow. The "driver" is already pretty simple in my opinion (see our Code), but is definetly optimizable.

Succressrate improvments

Hey @raultron,
we did some more benchmarking to learn about the origin of good and bad successrates. The findings are quite interesting.
We tested two different SPI-transfer-speeds (2 and 4 MHz) for three different message rates (250-1000Hz).
results

You can see that the success rate increases for higher SPI-transfer-speeds and slower message rates.
In addition to that we also tested our protocol with disabled additional interrupts (like HL - LL - communication). This results into 99,99% for Nucleo's receive and >90% for HLP's receive for nearly all message rates and SPI-transfer-speeds.

Interpretation:
Higher SPI-transfer-speeds for better successrates sounds not plausible after the first thought. But thought a little further it actually is. We tested different SPI-transfer-speeds for raw data transmission a few weeks ago and pointed out that we have a pretty good successrate at 4MHz. So the higher SPI-transfer-speed shouldn't lead to a bad successrate in our protocol, too. It just decreases the duration of transmission per message.
Also interesting is the benefit of slower message rates. In my opinion the HLP can just process a limited number of interrupts per timeslot. So decreasing the message rate leads directly to a lower number of interrupts per timeslot. Same effect shows the deactivation of background interrupts like the HL - LL - communication.

As we discussed this day, a great balance between SPI-transfer-speed and message rate is needed. We should not push a new message to the buffer until the last message was transmitted. If we do so, the buffer will overrun on the longer term.

Would love to hear your opinion on that points!

Have a good day everyone!

Buffers as FIFO or LIFO

@raultron @dinu @im-Kitsch
Just in case there is more than just one new (unread) package placed in the receive buffer:
Is there a reason to read older packages (with already outdated sensory/control data)? If not, I would prefer to use the buffer as a LIFO, so that we just pop the latest message out the buffer and discard the older ones. A LIFO has also the advantage that we don't have a problem with overflowing receive buffers.

PS: Currently we achieve pretty poor success rates with the interrupt based ringbuffer solution on the HLP (~20%). In most cases there is a single byte missing or the first X Bytes of the message are missing. We will try to find a solution for that problem, maybe someone have an idea how that comes?

Have a great day everyone!

EDIT: The Interrupt (ringbuffer) SPI on the Nucleo is working quite good btw. So we achieve pretty high success rates when using a blocking SPI solution on the HLP.

Notes of PX4FLOW

General Information of Sensor

Introduction

Important Information can be found mostly here. It's quite useful when use this sensor.

Using Ground Control Station To Get the Information

via: QGroundControl, here
via: MissionPlanner, here

Data Structure

There is 2 Data frame. I2C frame and I2C integral frame.
send 0x0 to PX4FLOW module and receive back 22 Bytes data, internal address auto increments
send 0x16 (22) to PX4FLOW module and receive back 25 Bytes data, internal address auto increments
Read data achievement is easy and achievement code is here.

    uint8_t *I2C_Receive = I2C_Info.arr;
    while ( HAL_I2C_Master_Transmit(&hi2c2, 0x84, send, 1, 0xff) != HAL_OK );
    while ( HAL_I2C_Master_Receive(&hi2c2, 0x84, I2C_Receive, 22,  0xfff) !=HAL_OK  );
    I2C_Receive = I2C_Integral_Info.arr;
    while ( HAL_I2C_Master_Transmit(&hi2c2, 0x84, receive, 1, 0xff) != HAL_OK );
    while ( HAL_I2C_Master_Receive(&hi2c2, 0x84, I2C_Receive, 25,  0xfff) !=HAL_OK  );

The Data Structure:

typedef struct i2c_frame
{
    uint16_t frame_count;// counts created I2C frames [#frames]
    int16_t pixel_flow_x_sum;// latest x flow measurement in pixels*10 [pixels]
    int16_t pixel_flow_y_sum;// latest y flow measurement in pixels*10 [pixels]
    int16_t flow_comp_m_x;// x velocity*1000 [meters/sec]
    int16_t flow_comp_m_y;// y velocity*1000 [meters/sec]
    int16_t qual;// Optical flow quality / confidence [0: bad, 255: maximum quality]
    int16_t gyro_x_rate; // latest gyro x rate [rad/sec]
    int16_t gyro_y_rate; // latest gyro y rate [rad/sec]
    int16_t gyro_z_rate; // latest gyro z rate [rad/sec]
    uint8_t gyro_range; // gyro range [0 .. 7] equals [50 deg/sec .. 2000 deg/sec] 
    uint8_t sonar_timestamp;// time since last sonar update [milliseconds]
    int16_t ground_distance;// Ground distance in meters*1000 [meters]. Positive value: distance known. Negative value: Unknown distance
} i2c_frame;
typedef struct i2c_integral_frame
{
    uint16_t frame_count_since_last_readout;//number of flow measurements since last I2C readout [#frames]
    int16_t pixel_flow_x_integral;//accumulated flow in radians*10000 around x axis since last I2C readout [rad*10000]
    int16_t pixel_flow_y_integral;//accumulated flow in radians*10000 around y axis since last I2C readout [rad*10000]
    int16_t gyro_x_rate_integral;//accumulated gyro x rates in radians*10000 since last I2C readout [rad*10000] 
    int16_t gyro_y_rate_integral;//accumulated gyro y rates in radians*10000 since last I2C readout [rad*10000] 
    int16_t gyro_z_rate_integral;//accumulated gyro z rates in radians*10000 since last I2C readout [rad*10000] 
    uint32_t integration_timespan;//accumulation timespan in microseconds since last I2C readout [microseconds]
    uint32_t sonar_timestamp;// time since last sonar update [microseconds]
    int16_t ground_distance;// Ground distance in meters*1000 [meters*1000]
    int16_t gyro_temperature;// Temperature * 100 in centi-degrees Celsius [degcelsius*100]
    uint8_t quality;// averaged quality of accumulated flow values [0:bad quality;255: max quality]
} __attribute__((packed)) i2c_integral_frame; 

Important Notes of Hardwire

The wire sequence of official document is wrong. It's also discussed in Internet.
The wire of official document is:
image
image

But the sequence is wrong. From Ground to 5V is not from 4 to 1 but 1 to 4.
It has been proved. See the corresponding cable sequence of Pixhawk!
image

Also the mentioned connector, DF13-4S-1.25C seems also not true. It can be found in inventory but doesn't match! Maybe Molex or some others......

Maximum power output of powerboard

We plan to use the 5V and GND output of the powerboard (Firefly) to power the Nucleo due to E5V - pin.
We assume that the maximum current of 500mA (2,5W) due to the E5V-pin is enough to power the Nucleoboard (See p. 20-24 in User Manual).
Unfortunatly we haven't found some information about the maximum power output of the powerboard yet.
If it isn't capable of powering the Nucleo, we have an alternative due to the Vin-pin, which allows 7V to 12V and can be directly connected to the battery (~11,1V).

SPI communication

We tested the SPI - communication between the Nucleo and HLP. The self-written driver on the HLP seems to work. But the builded cable is very fragile. New pre-crimped cables are already ordered with Dino's permission.
Byte transfers from Nucleo to HLP is working like expected. Byte transfers from HLP to Nucleo isn't working like expected (Nucleo just receives '0x00'). We looked at the signals with the oscilloscope and the Clock and SS is working like expected (if the clock isn't too high). The MISO and MOSI looks not clean (Up to four different voltage levels).

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. 📊📈🎉

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google ❤️ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.