EKF introduce ecl_float_t type for double precision floating point where available (disabled by default)

[dagar]

PX4/PX4-ECL#613

[dagar]

Seems to be working on fmu-v5. The individual double precision floating point operations aren't any slower, so I'm guessing the performance difference is largely from moving significantly more memory around. The size of the generated code also increased significantly.

PR

 PID COMMAND                   CPU(ms) CPU(%)  USED/STACK PRIO(BASE) STATE FD
   0 Idle Task                   22474 48.139   284/  748   0 (  0)  READY  3
   1 hpwork                          0  0.000   344/ 1780 249 (249)  w:sig  3
   2 lpwork                         33  0.080   616/ 1780  50 ( 50)  w:sig  8
   3 init                         1097  0.000  2040/ 2604 100 (100)  w:sem  3
   4 wq:manager                      2  0.000   808/ 1172 244 (244)  w:sem  7
 939 top                           851  2.993  1304/ 1684 255 (255)  RUN    3
  17 dataman                        81  0.000   752/ 1180  90 ( 90)  w:sem  4
  20 wq:lp_default                  39  0.080   848/ 1244 205 (205)  w:sem  7
  22 wq:I2C1                       265  0.485   824/ 1244 248 (248)  w:sem  7
  27 wq:hp_default                  99  0.080   832/ 1244 244 (244)  w:sem  7
 138 wq:SPI1                     10110 19.336   696/ 1244 254 (254)  READY  7
 143 wq:I2C2                         0  0.000   360/ 1244 247 (247)  w:sem  7
 149 wq:I2C3                       185  0.323   872/ 1244 246 (246)  READY  7
 152 wq:SPI5                         0  0.000   360/ 1244 250 (250)  w:sem  7
 165 wq:SPI4                       148  0.323   528/ 1244 251 (251)  READY  7
 183 sensors                      1361  2.588  1256/ 1964 238 (238)  READY 11
 186 commander                     428  0.566  1624/ 3212 140 (140)  READY  6
 188 commander_low_prio              1  0.000   576/ 2996  50 ( 50)  w:sem  6
 200 mavlink_if0                  2095  3.398  1648/ 2532 238 (100)  READY  4
 201 mavlink_rcv_if0               157  0.242  1424/ 2836 175 (175)  w:sem  4
 270 gps                           105  0.161  1096/ 1516 209 (209)  w:sem  4
 319 mavlink_if1                   965  1.132  1648/ 2484 100 (100)  w:sig  4
 326 mavlink_rcv_if1               181  0.242  1424/ 2836 175 (175)  w:sem  4
 364 px4io                        1011  2.022   928/ 1484 241 (241)  w:sem 11
 377 fmu                           188  0.404   888/ 1324 241 (241)  w:sem  7
 690 ekf2                         5744 11.407  8136/ 9572 239 (239)  w:sem  4
 701 mc_att_control               1282  2.588  1104/ 1660 240 (240)  w:sem  5
 711 mc_pos_control                140  0.242   944/ 1860 239 (239)  w:sem  4
 723 navigator                      71  0.161   888/ 1764 105 (105)  w:sem  4
 862 log_writer_file                 0  0.000   376/ 1164  60 ( 60)  w:sem 31
 845 logger                        301  0.566  1312/ 3644 234 (234)  READY 31

Processes: 31 total, 9 running, 22 sleeping, max FDs: 54
CPU usage: 49.43% tasks, 2.43% sched, 48.14% idle
DMA Memory: 5120 total, 1024 used 1024 peak
Uptime: 51.071s total, 22.475s idle

nsh> free
	     total       used       free    largest
Umem:       481360     204832     276528     274608

Master

 PID COMMAND                   CPU(ms) CPU(%)  USED/STACK PRIO(BASE) STATE FD
   0 Idle Task                   12006 53.117   284/  748   0 (  0)  READY  3
   1 hpwork                          0  0.000   344/ 1780 249 (249)  w:sig  3
   2 lpwork                         23  0.080   576/ 1780  50 ( 50)  w:sig  8
   3 init                         1096  0.000  2000/ 2604 100 (100)  w:sem  3
   4 wq:manager                      2  0.000   840/ 1172 244 (244)  w:sem  7
 944 top                           217  2.914  1304/ 1684 255 (255)  RUN    3
  17 dataman                        84  0.000   752/ 1180  90 ( 90)  w:sem  4
  20 wq:lp_default                  27  0.000   848/ 1244 205 (205)  w:sem  7
  22 wq:I2C1                       190  0.485   904/ 1244 248 (248)  READY  7
  27 wq:hp_default                  98  0.161   664/ 1244 244 (244)  READY  7
 138 wq:SPI1                      6894 19.028   696/ 1244 254 (254)  READY  7
 143 wq:I2C2                         0  0.000   360/ 1244 247 (247)  w:sem  7
 149 wq:I2C3                       131  0.323   776/ 1244 246 (246)  w:sem  7
 152 wq:SPI5                         0  0.000   360/ 1244 250 (250)  w:sem  7
 165 wq:SPI4                       102  0.323   528/ 1244 251 (251)  READY  7
 183 sensors                       974  2.591  1320/ 1964 238 (238)  w:sem 11
 186 commander                     388  0.566  1640/ 3212 234 (140)  READY  6
 188 commander_low_prio              0  0.000   560/ 2996  50 ( 50)  w:sem  6
 200 mavlink_if0                  1497  3.319  1648/ 2532 100 (100)  READY  4
 201 mavlink_rcv_if0               121  0.323  1424/ 2836 175 (175)  w:sem  4
 270 gps                            92  0.161  1096/ 1516 209 (209)  w:sem  4
 319 mavlink_if1                   888  1.133  1648/ 2484 105 (100)  w:sig  4
 326 mavlink_rcv_if1               143  0.323  1424/ 2836 175 (175)  w:sem  4
 364 px4io                         710  2.105   928/ 1484 241 (241)  w:sem 11
 377 fmu                           120  0.404   720/ 1324 241 (241)  w:sem  7
 690 ekf2                         2670  6.720  4520/ 6572 239 (239)  w:sem  4
 701 mc_att_control                945  2.834  1000/ 1660 240 (240)  w:sem  5
 711 mc_pos_control                113  0.242   944/ 1860 239 (239)  w:sem  4
 723 navigator                      57  0.161   888/ 1764 234 (105)  w:sem  4
 862 log_writer_file                 0  0.000   376/ 1164  60 ( 60)  w:sem 31
 845 logger                        249  0.485  1312/ 3644 234 (234)  w:sem 31

Processes: 31 total, 8 running, 23 sleeping, max FDs: 54
CPU usage: 44.70% tasks, 2.19% sched, 53.12% idle
DMA Memory: 5120 total, 1024 used 1024 peak
Uptime: 31.854s total, 12.007s idle

nsh> free
	     total       used       free    largest
Umem:       481360     196512     284848     279296

[dagar]

     VM SIZE                                                                                       FILE SIZE
 --------------                                                                                 --------------
  [NEW] +26.7Ki estimator::Ekf::predictCovariance()                                             +26.7Ki  [NEW]
  [NEW] +12.1Ki estimator::Ekf::fuseMag()                                                       +12.1Ki  [NEW]
  [NEW] +10.7Ki estimator::Ekf::fuseOptFlow()                                                   +10.7Ki  [NEW]
  [NEW] +6.32Ki estimator::Ekf::fuseDrag()                                                      +6.32Ki  [NEW]
  [NEW] +5.86Ki estimator::Ekf::fuseSideslip()                                                  +5.86Ki  [NEW]
  +1.7% +4.75Ki [321 Others]                                                                     +280Ki  +1.3%
  [NEW] +3.07Ki estimator::Ekf::fuseAirspeed()                                                  +3.07Ki  [NEW]
  [NEW] +2.46Ki estimator::Ekf::fuseHeading()                                                   +2.46Ki  [NEW]
  [NEW] +2.00Ki estimator::Ekf::controlFusionModes()                                            +2.00Ki  [NEW]
  [NEW] +1.72Ki estimator::Ekf::calculateOutputStates()                                         +1.72Ki  [NEW]
  [NEW] +1.69Ki estimator::Ekf::fuseVelPosHeight()                                              +1.69Ki  [NEW]
  [NEW] +1.64Ki estimator::Ekf::resetMagHeading(matrix::Vector3<double>&, bool, bool) (.part.6) +1.64Ki  [NEW]
  [NEW] +1.54Ki estimator::Ekf::controlMagFusion()                                              +1.54Ki  [NEW]
  [NEW] +1.54Ki estimator::Ekf::initialiseQuatCovariances(matrix::Vector3<double>&)             +1.54Ki  [NEW]
  [NEW] +1.46Ki estimator::Ekf::controlGpsFusion()                                              +1.46Ki  [NEW]
  [NEW] +1.44Ki estimator::Ekf::initialiseFilter()                                              +1.44Ki  [NEW]
  [NEW] +1.40Ki estimator::Ekf::fuseDeclination(float)                                          +1.40Ki  [NEW]
  [NEW] +1.36Ki estimator::Ekf::fuseGpsAntYaw()                                                 +1.36Ki  [NEW]
  [NEW] +1.36Ki estimator::Ekf::resetGpsAntYaw()                                                +1.36Ki  [NEW]
  [NEW] +1.30Ki estimator::EstimatorInterface::EstimatorInterface()                             +1.30Ki  [NEW]
  [NEW] +1.24Ki estimator::Ekf::controlHeightFusion()                                           +1.24Ki  [NEW]
  [NEW] +1.22Ki estimator::Ekf::controlHeightSensorTimeouts()                                   +1.22Ki  [NEW]
  [NEW] +1.21Ki estimator::Ekf::controlExternalVisionFusion() (.part.5)                         +1.21Ki  [NEW]
  [NEW] +1.15Ki estimator::Ekf::fixCovarianceErrors()                                           +1.15Ki  [NEW]
  [NEW] +1.14Ki estimator::Ekf::gps_is_good(estimator::gps_message const&)                      +1.14Ki  [NEW]
  [NEW] +1.11Ki estimator::Ekf::controlOpticalFlowFusion()                                      +1.11Ki  [NEW]
  [DEL] -1.10Ki Ekf::fixCovarianceErrors()                                                      -1.10Ki  [DEL]
  [DEL] -1.11Ki Ekf::controlOpticalFlowFusion()                                                 -1.11Ki  [DEL]
  [DEL] -1.13Ki Ekf::gps_is_good(estimator::gps_message const&)                                 -1.13Ki  [DEL]
  [DEL] -1.14Ki Ekf::controlHeightFusion()                                                      -1.14Ki  [DEL]
  [DEL] -1.19Ki Ekf::controlHeightSensorTimeouts()                                              -1.19Ki  [DEL]
  [DEL] -1.22Ki Ekf::fuseDeclination(float)                                                     -1.22Ki  [DEL]
  [DEL] -1.23Ki Ekf::fuseGpsAntYaw()                                                            -1.23Ki  [DEL]
  [DEL] -1.28Ki Ekf::controlExternalVisionFusion()                                              -1.28Ki  [DEL]
  [DEL] -1.29Ki Ekf::resetGpsAntYaw()                                                           -1.29Ki  [DEL]
  [DEL] -1.32Ki EstimatorInterface::EstimatorInterface()                                        -1.32Ki  [DEL]
  [DEL] -1.32Ki Ekf::initialiseQuatCovariances(matrix::Vector3<float>&)                         -1.32Ki  [DEL]
  [DEL] -1.40Ki Ekf::initialiseFilter()                                                         -1.40Ki  [DEL]
  [DEL] -1.42Ki Ekf::controlGpsFusion()                                                         -1.42Ki  [DEL]
  [DEL] -1.51Ki Ekf::controlMagFusion()                                                         -1.51Ki  [DEL]
  [DEL] -1.53Ki Ekf::resetMagHeading(matrix::Vector3<float>&, bool, bool) (.part.5)             -1.53Ki  [DEL]
  [DEL] -1.54Ki Ekf::calculateOutputStates()                                                    -1.54Ki  [DEL]
  [DEL] -1.60Ki Ekf::fuseVelPosHeight()                                                         -1.60Ki  [DEL]
  [DEL] -1.98Ki Ekf::controlFusionModes()                                                       -1.98Ki  [DEL]
  [DEL] -2.11Ki Ekf::fuseHeading()                                                              -2.11Ki  [DEL]
  [DEL] -2.72Ki Ekf::fuseAirspeed()                                                             -2.72Ki  [DEL]
  [DEL] -5.02Ki Ekf::fuseDrag()                                                                 -5.02Ki  [DEL]
  [DEL] -5.30Ki Ekf::fuseSideslip()                                                             -5.30Ki  [DEL]
  [DEL] -8.50Ki Ekf::fuseOptFlow()                                                              -8.50Ki  [DEL]
  [DEL] -11.0Ki Ekf::fuseMag()                                                                  -11.0Ki  [DEL]
  [DEL] -20.7Ki Ekf::predictCovariance()                                                        -20.7Ki  [DEL]
  +1.1% +17.8Ki TOTAL                                                                            +293Ki  +1.3%

[dagar]

@PX4/testflights this is lower priority, but could you give this PR an initial flight test on any fmu-v5 (pixhawk 4 or 4 mini)?
Do a sanity check on the ground first and make sure the vehicle's attitude, heading, and position look correct in QGC.

[Junkim3DR]

Tested on Pixhawk 4 mini v5

Modes Tested

• Position Mode: Started going forward by his own, can be controlled but does not stay in place..
• Altitude Mode: Good.
• Stabilized Mode: Good.
• Mission Plan Mode (Automated): Not performing as expected, flew freely and randomly in the air.
• RTL: Not performing as expected.

Procedure
Arm and Take off in stabilized mode, after flying for approximately one minute, switched to altitude then position mode proceed to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC (Vehicle flew randomly did not follow waypoints), once completed all waypoint activate (None completed) RTL (Did not land, landed manually in stabilized mode) and see landing behaviour.

Notes:
Position, Mission Plan Mode (Automates) and RTL (automated) were not working as they should.

• Position: Did not stay in place, kept going forward alone.
• Mission Plan: Does not do mission, it flies randomly and does not follow waypoints.
• RTL: Stayed flying randomly in the air and had to land manually on stabilized mode.

Logs:
PR 12215

• https://review.px4.io/plot_app?log=7206dc79-eb87-40ce-a270-7697abd57a9a

Master

• https://review.px4.io/plot_app?log=b014c23c-c92a-45fd-8bc7-062bac5d909e

[dannyfpv]

Tested on Pixhawk 4 v5

Modes tested:

• Altitude Mode: Good.
• Stabilized Mode: Good.
• Position Mode: Started going forward on its own
• Mission Plan Mode (Automated): Flew freely and randomly in the air.
• RTL (Automated): Flew randomly in the air, did not start landing procedure.

Procedure:
Armed and took off in stabilized mode, switched to altitude mode then and position (Position Mode did not perform correctly), proceeded to trigger mission plan mode (vehicle flew freely and randomly in the air and did not follow the waypoints), RTL (had the same issue as mission mode).

Notes:
Position, Mission Plan Mode (Automated) and RTL (Automated) were not working as they should.
Position: Did not stay in place, kept going forward alone.

PR Log:
https://review.px4.io/plot_app?log=388f7340-65cb-429b-b41d-a8d809dc9d34

Master log:
https://review.px4.io/plot_app?log=71a5aa74-1e6e-4c9c-ae20-2baff55f3daf

[jorge789]

Tested on Pixhawk 2 Cube V3:

Note:
Loaded PR 12215, took off in stabilized mode, proceeded to climb up to about 10 meters approximately, the vehicle began to fly sideways randomly, then the user began to land but due to the sideways movement vehicle crashed breaking off one arm (User tried to compensate the movement but due to it being so random wasn't able to land successfully).

Log
https://review.px4.io/plot_app?log=a7a4af4d-4bbf-430b-bbcd-3c546345c5fc

Note
This flight is from 6/10/2019

[julianoes]

Nice.

[stale]

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.

[julianoes]

Any plans here @dagar?

[stale]

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.

[stale]

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.