toppers / hakoniwa-px4sim

PX4-compatible drone simulation with physics-based modeling in C, visualizations via game engines, headless operation, and automated test scenarios. Supports external parameterization and MATLAB/Simulink integration.

40 stars 11 forks source link

地上座標系と機体座標系の物理モデルの実験をやって比較する #123

Closed tmori closed 8 months ago

tmori commented 10 months ago

機体座標系の結果

alt_result-p6000-p0_11.txt

drone_dynamics-p6000-p0.11.csv

tmori@TakashinoMacBook-Pro hakoniwa-px4sim % bash px4/auto-test/auto-test.bash ./px4/auto-test/test_resource ./px4/auto-test/test_scenario/takeoff_and_move.json
INFO: START TEST PARAM=6000-0.11
INFO: Activating hakoniwa
HAKO_CAPTURE_SAVE_FILEPATH : ./capture.bin
HAKO_BYPASS_IPADDR : 127.0.0.1
HAKO_CUSTOM_JSON_PATH : ../config/custom.json
DRONE_CONFIG_PATH : /Users/tmori/project/oss/hakoniwa-px4sim/./px4/auto-test/test_resource/work/drone_config.json
HAKO_BYPASS_PORTNO : 54001
INFO: shmget() key=255 size=1129352 
INFO: hako_master_init() success
Robot: DroneAvator, PduWriter: DroneAvator_drone_motor
channel_id: 0 pdu_size: 88
INFO: DroneAvator create_lchannel: logical_id=0 real_id=0 size=88
Robot: DroneAvator, PduWriter: DroneAvator_drone_pos
channel_id: 1 pdu_size: 48
INFO: DroneAvator create_lchannel: logical_id=1 real_id=1 size=48
WAIT START
INFO: Activating PX4
ps: stdin: not a terminal
[0/1] launching px4 none_iris (SYS_AUTOSTART=10016)

______  __   __    ___ 
| ___ \ \ \ / /   /   |
| |_/ /  \ V /   / /| |
|  __/   /   \  / /_| |
| |     / /^\ \ \___  |
\_|     \/   \/     |_/

px4 starting.

INFO  [px4] startup script: /bin/sh etc/init.d-posix/rcS 0
env SYS_AUTOSTART: 10016
INFO  [param] selected parameter default file parameters.bson
INFO  [param] importing from 'parameters.bson'
INFO  [parameters] BSON document size 971 bytes, decoded 971 bytes (INT32:15, FLOAT:29)
INFO  [param] selected parameter backup file parameters_backup.bson
INFO  [dataman] data manager file './dataman' size is 7868392 bytes
INFO  [init] PX4_SIM_HOSTNAME: localhost
INFO  [simulator_mavlink] Waiting for simulator to accept connection on TCP port 4560
INFO  [simulator_mavlink] Simulator connected on TCP port 4560.
INFO: px4 reciver start
INFO: COMMAND_LONG ack sended
ERROR [simulator_mavlink] poll timeout 0, 22
ERROR [simulator_mavlink] poll timeout 0, 22
INFO: start simulation
start
WAIT RUNNING
PDU CREATED
START CREATE PDU DATA: total_size= 136
INFO: shmget() key=256 size=136 
PDU DATA CREATED
CREATED ADDR=0x10104400c
INFO: setup start
param_A: 1.53281e-08
param_B: 3e-09
param_Jr: 2.5e-07
INFO: setup done
INFO: start simulation
SYNC MODE: true
Robot: DroneAvator, PduWriter: DroneAvator_drone_motor
channel_id: 0 pdu_size: 88
Robot: DroneAvator, PduWriter: DroneAvator_drone_pos
channel_id: 1 pdu_size: 48
INFO  [lockstep_scheduler] setting initial absolute time to 1704787303798330 us
WARN  [vehicle_angular_velocity] no gyro selected, using sensor_gyro_fifo:0 1310988
INFO  [commander] LED: open /dev/led0 failed (22)
WARN  [health_and_arming_checks] Preflight Fail: ekf2 missing data
INFO  [tone_alarm] home set
WARN  [health_and_arming_checks] Preflight Fail: ekf2 missing data
me:192.168.11.33:54001
px4:127.0.0.1:18570
qgc:192.168.11.56:14550
INFO: takeoff operation alt: 131.321
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550
INFO  [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540
INFO  [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14280 remote port 14030
INFO  [mavlink] mode: Gimbal, data rate: 400000 B/s on udp port 13030 remote port 13280
INFO  [logger] logger started (mode=all)
INFO  [logger] Start file log (type: full)
INFO  [logger] [logger] ./log/2024-01-09/08_02_00.ulg   
INFO  [logger] Opened full log file: ./log/2024-01-09/08_02_00.ulg
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [px4] Startup script returned successfully
pxh> INFO: Takeoff Operation started.
INFO  [commander] Ready for takeoff!
INFO  [mavlink] partner IP: 127.0.0.1
pxh> COMMAND_LONG dump:
Header:
pxh>   Message Type: COMMAND_LONG
  System ID: 255
  Component ID: 190
  Command: 22
pxh>   Param1: -1.0
pxh>   Param2: 0.0
pxh>   Param3: 0.0
  Param4: nan
  Param5: nan
pxh>   Param6: nan
pxh>   Param7: 131.321
  Target System: 1
  Target Component: 1
  Confirmation: 0
pxh> INFO: takeoff event start_takeoff
pxh> PX4: 4025045:COMMAND_ACK
COMMAND_LONG dump:
Header:
  Message Type: COMMAND_LONG
  System ID: 255
  Component ID: 190
  Command: 400
  Param1: 1.0
  Param2: 0.0
  Param3: 0.0
  Param4: 0.0
  Param5: 0.0
pxh>   Param6: 0.0
pxh>   Param7: 0.0
pxh>   Target System: 1
  Target Component: 1
  Confirmation: 0
pxh> INFO: takeoff event start_arm
pxh> INFO  [commander] Armed by external command    
pxh> INFO  [tone_alarm] arming warning
pxh> PX4: 5040071:COMMAND_ACK
pxh> INFO  [commander] Takeoff detected 
INFO: Takeoff Operation done.
INFO: start_landing operation lat: 476414680 lon: -1221405253 alt: 131.321
pxh> INFO: StartLanding Operation started.
pxh> COMMAND_INT dump:
pxh> Header:
  Message Type: COMMAND_INT
  System ID: 255
  Component ID: 190
  Command: 192
  Param1: 0
  Param2: 0
pxh>   Param3: 0
  Param4: nan
pxh>   X: 476414680
pxh>   Y: -1221405253
pxh>   Z: 131.321
  Target System: 1
  Target Component: 1
  Frame: 0
pxh>   Current: 0
  Autocontinue: 0
pxh> INFO: landing event start_landing
pxh> INFO  [tone_alarm] notify negative
pxh> PX4: 34133900:COMMAND_ACK
pxh> WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
pxh> WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
INFO: StartLanding Operation done.
pxh> INFO: start_landing operation lat: 476418283 lon: -1221401650 alt: 131.321
INFO: StartLanding Operation started.
COMMAND_INT dump:
pxh> Header:
  Message Type: COMMAND_INT
pxh>   System ID: 255
pxh>   Component ID: 190
  Command: 192
  Param1: 0
pxh>   Param2: 0
  Param3: 0
pxh>   Param4: nan
pxh>   X: 476418283
  Y: -1221401650
pxh>   Z: 131.321
pxh>   Target System: 1
  Target Component: 1
pxh>   Frame: 0
  Current: 0
pxh>   Autocontinue: 0
pxh> INFO: landing event start_landing
PX4: 93367323:COMMAND_ACK
pxh> INFO  [tone_alarm] notify negative
WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
INFO: StartLanding Operation done.
INFO: start_landing operation lat: 476414680 lon: -1221401650 alt: 131.321
pxh> INFO: StartLanding Operation started.
COMMAND_INT dump:
Header:
pxh>   Message Type: COMMAND_INT
  System ID: 255
  Component ID: 190
  Command: 192
  Param1: 0
pxh>   Param2: 0
  Param3: 0
  Param4: nan
  X: 476414680
  Y: -1221401650
pxh>   Z: 131.321
  Target System: 1
  Target Component: 1
pxh>   Frame: 0
pxh>   Current: 0
  Autocontinue: 0
INFO: landing event start_landing
pxh> INFO  [tone_alarm] notify negative
pxh> PX4: 152603081:COMMAND_ACK
WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
pxh> INFO: StartLanding Operation done.
pxh> INFO: start_landing operation lat: 476418283 lon: -1221401650 alt: 131.321
pxh> INFO: StartLanding Operation started.
COMMAND_INT dump:
Header:
pxh>   Message Type: COMMAND_INT
  System ID: 255
  Component ID: 190
  Command: 192
pxh>   Param1: 0
  Param2: 0
pxh>   Param3: 0
  Param4: nan
  X: 476418283
  Y: -1221401650
pxh>   Z: 131.321
pxh>   Target System: 1
pxh>   Target Component: 1
pxh>   Frame: 0
pxh>   Current: 0
pxh>   Autocontinue: 0
pxh> INFO: landing event start_landing
pxh> INFO  [tone_alarm] notify negative
PX4: 211854146:COMMAND_ACK
WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
pxh> INFO: StartLanding Operation done.
INFO: All Operations done.
pxh> INFO: Program is exiting.
INFO: stop simulation
INFO: Stopping processes...
pxh> kill -TERM 68065
kill -TERM 68067
make: *** [px4_sitl] Killed: 9
INFO: All processes stopped.
FAILED: src/modules/simulation/simulator_mavlink/CMakeFiles/none_iris /Users/tmori/project/oss/hakoniwa-px4sim/px4/PX4-Autopilot/build/px4_sitl_default/src/modules/simulation/simulator_mavlink/CMakeFiles/none_iris 
cd /Users/tmori/project/oss/hakoniwa-px4sim/px4/PX4-Autopilot/build/px4_sitl_default/src/modules/simulation/simulator_mavlink && /opt/homebrew/Cellar/cmake/3.27.7/bin/cmake -E env PX4_SYS_AUTOSTART=10016 /Users/tmori/project/oss/hakoniwa-px4sim/px4/PX4-Autopilot/build/px4_sitl_default/bin/px4
ninja: build stopped: subcommand failed.
relative_start_time: 209.782
relative_end_time: 214.782
actual_mean: -9.988396025194962
actual_stdev: 0.005769714754517385
mean_score: 99.32900016882299
upper_score: 98.50210628391875
lower_score: 99.82995179439104
total_score: 99.22035274904425
Score: 99.22035274904425
relative_start_time: 209.782
relative_end_time: 214.782
actual_mean: 39.8975065794841
actual_stdev: 0.13250369099118744
mean_score: 59.14104141201262
upper_score: 95.59680215994419
lower_score: 6.321699404650835
total_score: 53.68651432553588
Score: 53.68651432553588
relative_start_time: 209.782
relative_end_time: 214.782
actual_mean: -0.010430932213557294
actual_stdev: 0.08449902642519523
mean_score: 99.4574553841884
upper_score: 76.01008713813732
lower_score: 63.72553423237463
total_score: 79.73102558490012
Score: 79.73102558490012
INFO: END TEST

tmori commented 10 months ago

地上座標家の結果

alt_result-p6000-p0_11.txt

drone_dynamics-p6000-p0.11.csv

tmori@TakashinoMacBook-Pro hakoniwa-px4sim % bash px4/auto-test/auto-test.bash ./px4/auto-test/test_resource ./px4/auto-test/test_scenario/takeoff_and_move.json
INFO: START TEST PARAM=6000-0.11
INFO: Activating hakoniwa
HAKO_CAPTURE_SAVE_FILEPATH : ./capture.bin
HAKO_BYPASS_IPADDR : 127.0.0.1
HAKO_CUSTOM_JSON_PATH : ../config/custom.json
DRONE_CONFIG_PATH : /Users/tmori/project/oss/hakoniwa-px4sim/./px4/auto-test/test_resource/work/drone_config.json
HAKO_BYPASS_PORTNO : 54001
INFO: shmget() key=255 size=1129352 
INFO: hako_master_init() success
Robot: DroneAvator, PduWriter: DroneAvator_drone_motor
channel_id: 0 pdu_size: 88
INFO: DroneAvator create_lchannel: logical_id=0 real_id=0 size=88
Robot: DroneAvator, PduWriter: DroneAvator_drone_pos
channel_id: 1 pdu_size: 48
INFO: DroneAvator create_lchannel: logical_id=1 real_id=1 size=48
WAIT START
INFO: Activating PX4
ps: stdin: not a terminal
[0/1] launching px4 none_iris (SYS_AUTOSTART=10016)

______  __   __    ___ 
| ___ \ \ \ / /   /   |
| |_/ /  \ V /   / /| |
|  __/   /   \  / /_| |
| |     / /^\ \ \___  |
\_|     \/   \/     |_/

px4 starting.

INFO  [px4] startup script: /bin/sh etc/init.d-posix/rcS 0
env SYS_AUTOSTART: 10016
INFO  [param] selected parameter default file parameters.bson
INFO  [param] importing from 'parameters.bson'
INFO  [parameters] BSON document size 971 bytes, decoded 971 bytes (INT32:15, FLOAT:29)
INFO  [param] selected parameter backup file parameters_backup.bson
INFO  [dataman] data manager file './dataman' size is 7868392 bytes
INFO  [init] PX4_SIM_HOSTNAME: localhost
INFO  [simulator_mavlink] Waiting for simulator to accept connection on TCP port 4560
INFO  [simulator_mavlink] Simulator connected on TCP port 4560.
INFO: px4 reciver start
INFO: COMMAND_LONG ack sended
ERROR [simulator_mavlink] poll timeout 0, 22
ERROR [simulator_mavlink] poll timeout 0, 22
INFO: start simulation
start
WAIT RUNNING
PDU CREATED
START CREATE PDU DATA: total_size= 136
INFO: shmget() key=256 size=136 
PDU DATA CREATED
CREATED ADDR=0x10515c00c
INFO: setup start
param_A: 1.53281e-08
param_B: 3e-09
param_Jr: 2.5e-07
INFO: setup done
INFO: start simulation
SYNC MODE: true
Robot: DroneAvator, PduWriter: DroneAvator_drone_motor
channel_id: 0 pdu_size: 88
Robot: DroneAvator, PduWriter: DroneAvator_drone_pos
channel_id: 1 pdu_size: 48
INFO  [lockstep_scheduler] setting initial absolute time to 1704786879590262 us
WARN  [vehicle_angular_velocity] no gyro selected, using sensor_gyro_fifo:0 1310988
INFO  [commander] LED: open /dev/led0 failed (22)
WARN  [health_and_arming_checks] Preflight Fail: ekf2 missing data
INFO  [tone_alarm] home set
WARN  [health_and_arming_checks] Preflight Fail: ekf2 missing data
me:192.168.11.33:54001
px4:127.0.0.1:18570
qgc:192.168.11.56:14550
INFO: takeoff operation alt: 131.321
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550
INFO  [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540
INFO  [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14280 remote port 14030
INFO  [mavlink] mode: Gimbal, data rate: 400000 B/s on udp port 13030 remote port 13280
INFO  [logger] logger started (mode=all)
INFO  [logger] Start file log (type: full)
INFO  [logger] [logger] ./log/2024-01-09/07_54_57.ulg   
INFO  [logger] Opened full log file: ./log/2024-01-09/07_54_57.ulg
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [px4] Startup script returned successfully
pxh> INFO: Takeoff Operation started.
pxh> INFO  [mavlink] partner IP: 127.0.0.1
pxh> INFO  [commander] Ready for takeoff!
COMMAND_LONG dump:
Header:
pxh>   Message Type: COMMAND_LONG
  System ID: 255
  Component ID: 190
  Command: 22
  Param1: -1.0
  Param2: 0.0
pxh>   Param3: 0.0
pxh>   Param4: nan
  Param5: nan
  Param6: nan
pxh>   Param7: 131.321
  Target System: 1
pxh>   Target Component: 1
  Confirmation: 0
INFO: takeoff event start_takeoff
pxh> PX4: 4035959:COMMAND_ACK
pxh> COMMAND_LONG dump:
Header:
pxh>   Message Type: COMMAND_LONG
  System ID: 255
  Component ID: 190
pxh>   Command: 400
pxh>   Param1: 1.0
pxh>   Param2: 0.0
pxh>   Param3: 0.0
  Param4: 0.0
  Param5: 0.0
  Param6: 0.0
pxh>   Param7: 0.0
pxh>   Target System: 1
pxh>   Target Component: 1
  Confirmation: 0
INFO: takeoff event start_arm
pxh> INFO  [commander] Armed by external command    
pxh> INFO  [tone_alarm] arming warning
pxh> PX4: 5035155:COMMAND_ACK
INFO  [commander] Takeoff detected  
INFO: Takeoff Operation done.
pxh> INFO: start_landing operation lat: 476414680 lon: -1221405253 alt: 131.321
pxh> INFO: StartLanding Operation started.
pxh> COMMAND_INT dump:
pxh> Header:
  Message Type: COMMAND_INT
  System ID: 255
pxh>   Component ID: 190
pxh>   Command: 192
pxh>   Param1: 0
  Param2: 0
pxh>   Param3: 0
  Param4: nan
  X: 476414680
  Y: -1221405253
  Z: 131.321
pxh>   Target System: 1
  Target Component: 1
pxh>   Frame: 0
  Current: 0
  Autocontinue: 0
pxh> INFO: landing event start_landing
pxh> INFO  [tone_alarm] notify negative
pxh> PX4: 34158092:COMMAND_ACK
pxh> WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
pxh> WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
WARN  [health_and_arming_checks] Compass needs calibration - Land now!  
pxh> WARN  [health_and_arming_checks] Compass needs calibration - Land now! 
WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
pxh> WARN  [health_and_arming_checks] Compass needs calibration - Land now! 
pxh> WARN  [health_and_arming_checks] Compass needs calibration - Land now! 
WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
pxh> WARN  [health_and_arming_checks] Compass needs calibration - Land now! 
WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
pxh> WARN  [health_and_arming_checks] Compass needs calibration - Land now! 
pxh> INFO: StartLanding Operation done.
INFO: start_landing operation lat: 476418283 lon: -1221401650 alt: 131.321
INFO: StartLanding Operation started.
COMMAND_INT dump:
Header:
  Message Type: COMMAND_INT
  System ID: 255
  Component ID: 190
  Command: 192
  Param1: 0
  Param2: 0
  Param3: 0
  Param4: nan
  X: 476418283
  Y: -1221401650
  Z: 131.321
  Target System: 1
  Target Component: 1
  Frame: 0
  Current: 0
  Autocontinue: 0
INFO: landing event start_landing
pxh> INFO  [tone_alarm] notify negative
pxh> PX4: 93407096:COMMAND_ACK
pxh> INFO: StartLanding Operation done.
INFO: start_landing operation lat: 476414680 lon: -1221401650 alt: 131.321
pxh> INFO: StartLanding Operation started.
pxh> COMMAND_INT dump:
Header:
pxh>   Message Type: COMMAND_INT
  System ID: 255
  Component ID: 190
  Command: 192
pxh>   Param1: 0
  Param2: 0
pxh>   Param3: 0
  Param4: nan
pxh>   X: 476414680
  Y: -1221401650
  Z: 131.321
pxh>   Target System: 1
  Target Component: 1
pxh>   Frame: 0
  Current: 0
  Autocontinue: 0
INFO: landing event start_landing
pxh> INFO  [tone_alarm] notify negative
PX4: 152642484:COMMAND_ACK
WARN  [health_and_arming_checks] Compass needs calibration - Land now!  
WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
pxh> WARN  [health_and_arming_checks] Compass needs calibration - Land now! 
pxh> INFO: StartLanding Operation done.
INFO: start_landing operation lat: 476418283 lon: -1221401650 alt: 131.321
INFO: StartLanding Operation started.
COMMAND_INT dump:
pxh> Header:
  Message Type: COMMAND_INT
pxh>   System ID: 255
  Component ID: 190
pxh>   Command: 192
  Param1: 0
  Param2: 0
  Param3: 0
pxh>   Param4: nan
pxh>   X: 476418283
pxh>   Y: -1221401650
  Z: 131.321
  Target System: 1
  Target Component: 1
  Frame: 0
pxh>   Current: 0
  Autocontinue: 0
INFO: landing event start_landing
pxh> PX4: 211881771:COMMAND_ACK
pxh> INFO  [tone_alarm] notify negative
pxh> WARN  [health_and_arming_checks] Compass needs calibration - Land now! 
WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
pxh> WARN  [health_and_arming_checks] Compass needs calibration - Land now! 
pxh> INFO: StartLanding Operation done.
INFO: All Operations done.
INFO: Program is exiting.
pxh> INFO: stop simulation
pxh> INFO: Stopping processes...
kill -TERM 67295
pxh> kill -TERM 67298
pxh> make: *** [px4_sitl] Killed: 9
INFO: All processes stopped.
FAILED: src/modules/simulation/simulator_mavlink/CMakeFiles/none_iris /Users/tmori/project/oss/hakoniwa-px4sim/px4/PX4-Autopilot/build/px4_sitl_default/src/modules/simulation/simulator_mavlink/CMakeFiles/none_iris 
cd /Users/tmori/project/oss/hakoniwa-px4sim/px4/PX4-Autopilot/build/px4_sitl_default/src/modules/simulation/simulator_mavlink && /opt/homebrew/Cellar/cmake/3.27.7/bin/cmake -E env PX4_SYS_AUTOSTART=10016 /Users/tmori/project/oss/hakoniwa-px4sim/px4/PX4-Autopilot/build/px4_sitl_default/bin/px4
ninja: build stopped: subcommand failed.
relative_start_time: 209.941
relative_end_time: 214.941
actual_mean: -10.005359398920215
actual_stdev: 0.007141768183417443
mean_score: 99.8564872941856
upper_score: 99.98411706052033
lower_score: 99.2216490667273
total_score: 99.68741780714441
Score: 99.68741780714441
relative_start_time: 209.941
relative_end_time: 214.941
actual_mean: 39.96106124475104
actual_stdev: 0.06331454379536902
mean_score: 92.69910826731108
upper_score: 97.07280195637702
lower_score: 59.28660100151832
total_score: 83.01950374173548
Score: 83.01950374173548
relative_start_time: 209.941
relative_end_time: 214.941
actual_mean: -0.02435073125374925
actual_stdev: 0.0711580020370596
mean_score: 97.07872821179781
upper_score: 89.62409095250347
lower_score: 63.37530529940444
total_score: 83.35937482123524
Score: 83.35937482123524
INFO: END TEST

kenjihiranabe commented 10 months ago

ぴったり一緒とは行かないみたいだけど，まあ，数値計算の誤差程度に思っておきますかね？

tmori commented 10 months ago

比較結果

ドローンの位置はほぼ同じ動きをしている。（シミュレーション結果はパソコンの負荷によって変わるので仕方ない）

１点記になるのは、地上座標系のPX4のログでは、以下の警告が発生している点。（機体座標系の場合は出力されていない）

WARN  [health_and_arming_checks] Compass needs calibration - Land now!

kenjihiranabe commented 10 months ago

gyro がうまく同期していない？

tmori commented 10 months ago

gyroとmagのセンシングデータをもう一度採取してみます。

tmori commented 10 months ago

機体座標系の全データ

logs_body.zip

drone_dynamics rx ry rz

mag

gyro

tmori commented 10 months ago

地上座表系の全データ

[Uploading logs_ground.zip…]()

drone_dynamics rx ry rz

mag

gyro

tmori commented 10 months ago

地上座標系と機体座標系とで Rzの値が全然違う。真逆？

kenjihiranabe commented 10 months ago

@tmori said ... 初期の方向は東方向に向いていて、ミッションとして、真西に移動させるので、方向転換するのですが、機体座標系の場合と地上座標系とで、選択した旋回方向が違うようですね。

kenjihiranabe commented 10 months ago

選択の違いであれば，分からないですね．．．．でもエラーは気になる．

tmori commented 10 months ago

選択の違いであれば，分からないですね．．．．でもエラーは気になる．

ですよねぇ。明日、もう少し調べてみます。機体座標系で同じ選択をした場合、同じ結果になるかどうかですね。（逆も然り）

tmori commented 10 months ago

再チャレンジ

rotor_thruster：nonlinear パラメータ：Kr=6000, Tr=0.11

コンフィグ：

{
  "simulation": {
    "lockstep": true,
    "timeStep": 0.003,
    "logOutputDirectory": "/Users/tmori/project/oss/hakoniwa-px4sim/px4/auto-test/test_resource/logs/",
    "logOutput": {
      "sensors": {
          "acc": true,
          "gyro": true,
          "mag": true,
          "baro": true,
          "gps": true    
      },
      "mavlink": {
          "hil_sensor": true,
          "hil_gps": true,
          "hil_actuator_controls": true
      }
    },
    "mavlink_tx_period_msec": {
      "hil_sensor": 3,
      "hil_gps": 3
    },
    "location": {
      "latitude": 47.641468,
      "longitude": -122.140165,
      "altitude": 121.321,
      "magneticField": {
        "intensity_nT": 53045.1,
        "declination_deg": 15.306,
        "inclination_deg": 68.984
      }  
    }
  },
  "components": {
      "droneDynamics": {
          "physicsEquation": "BodyFrame",
          "collision_detection": false,
          "airFrictionCoefficient": 0.0001,
          "inertia": [ 0.0000625, 0.00003125, 0.00009375 ],
          "mass_kg": 0.1,
          "body_size": [ 0.1, 0.1, 0.01 ],
          "position_meter": [ 0, 0, 0 ],
          "angle_degree": [ 0, 0, 90 ]
        },
        "rotor": {
          "vendor": "None",
          "Tr": 0.11,
          "Kr": 6000,
          "rpmMax": 8000
        },
        "thruster": {
          "vendor": "None",
          "rotorPositions": [
            {"position": [  0.05,  0.05, 0 ], "rotationDirection":  1.0 },
            {"position": [ -0.05, -0.05, 0 ], "rotationDirection":  1.0 },
            {"position": [  0.05, -0.05, 0 ], "rotationDirection": -1.0 },
            {"position": [ -0.05,  0.05, 0 ], "rotationDirection": -1.0 }
            ],
          "HoveringRpm": 4000,
          "parameterB":  1.3e-10,
          "parameterJr": 6.0e-10
        },
        "sensors": {
          "acc": {
            "sampleCount": 1,
            "noise": 0.03
          },
          "gyro": {
            "sampleCount": 1,
            "noise": 0.01
          },
          "mag": {
            "sampleCount": 1,
            "noise": 0.03
          },
          "baro": {
            "sampleCount": 1,
            "noise": 0.01
          },
          "gps": {
            "sampleCount": 1,
            "noise": 0
          }
        }
  }
}

tmori commented 10 months ago

機体座標系

compass 系のWARNINGが最後の方に出ているけど、まー、たまには出る。

tmori@TakashinoMacBook-Pro hakoniwa-px4sim % bash px4/auto-test/auto-test.bash ./px4/auto-test/test_resource ./px4/auto-test/test_scenario/takeoff_and_move.json
INFO: START TEST PARAM=6000-0.11
INFO: Activating hakoniwa
HAKO_CAPTURE_SAVE_FILEPATH : ./capture.bin
HAKO_BYPASS_IPADDR : 127.0.0.1
HAKO_CUSTOM_JSON_PATH : ../config/custom.json
DRONE_CONFIG_PATH : /Users/tmori/project/oss/hakoniwa-px4sim/./px4/auto-test/test_resource/work/drone_config.json
HAKO_BYPASS_PORTNO : 54001
INFO: shmget() key=255 size=1129352 
INFO: hako_master_init() success
Robot: DroneAvator, PduWriter: DroneAvator_drone_motor
channel_id: 0 pdu_size: 88
INFO: DroneAvator create_lchannel: logical_id=0 real_id=0 size=88
Robot: DroneAvator, PduWriter: DroneAvator_drone_pos
channel_id: 1 pdu_size: 48
INFO: DroneAvator create_lchannel: logical_id=1 real_id=1 size=48
WAIT START
INFO: Activating PX4
ps: stdin: not a terminal
[0/1] launching px4 none_iris (SYS_AUTOSTART=10016)

______  __   __    ___ 
| ___ \ \ \ / /   /   |
| |_/ /  \ V /   / /| |
|  __/   /   \  / /_| |
| |     / /^\ \ \___  |
\_|     \/   \/     |_/

px4 starting.

INFO  [px4] startup script: /bin/sh etc/init.d-posix/rcS 0
env SYS_AUTOSTART: 10016
INFO  [param] selected parameter default file parameters.bson
INFO  [param] importing from 'parameters.bson'
INFO  [parameters] BSON document size 971 bytes, decoded 971 bytes (INT32:15, FLOAT:29)
INFO  [param] selected parameter backup file parameters_backup.bson
INFO  [dataman] data manager file './dataman' size is 7868392 bytes
INFO  [init] PX4_SIM_HOSTNAME: localhost
INFO  [simulator_mavlink] Waiting for simulator to accept connection on TCP port 4560
INFO  [simulator_mavlink] Simulator connected on TCP port 4560.
INFO: px4 reciver start
INFO: COMMAND_LONG ack sended
ERROR [simulator_mavlink] poll timeout 0, 22
ERROR [simulator_mavlink] poll timeout 0, 22
INFO: start simulation
start
WAIT RUNNING
START CREATE PDU DATA: total_size= 136
INFO: shmget() key=256 size=136 
PDU DATA CREATED
CREATED ADDR=0x100bfc00c
PDU CREATED
INFO: setup start
Rotor vendor: None
Thruster vendor: None
param_A: 1.53281e-08
param_B: 1.3e-10
param_Jr: 6e-10
INFO: setup done
INFO: start simulation
SYNC MODE: true
Robot: DroneAvator, PduWriter: DroneAvator_drone_motor
channel_id: 0 pdu_size: 88
Robot: DroneAvator, PduWriter: DroneAvator_drone_pos
channel_id: 1 pdu_size: 48
INFO  [lockstep_scheduler] setting initial absolute time to 1704957293538431 us
WARN  [vehicle_angular_velocity] no gyro selected, using sensor_gyro_fifo:0 1310988
INFO  [commander] LED: open /dev/led0 failed (22)
WARN  [health_and_arming_checks] Preflight Fail: ekf2 missing data
INFO  [tone_alarm] home set
WARN  [health_and_arming_checks] Preflight Fail: ekf2 missing data
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550
me:192.168.11.33:54001
px4:127.0.0.1:18570
qgc:192.168.11.56:14550
INFO: takeoff operation alt: 131.321
INFO  [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540
INFO  [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14280 remote port 14030
INFO  [mavlink] mode: Gimbal, data rate: 400000 B/s on udp port 13030 remote port 13280
INFO  [logger] logger started (mode=all)
INFO  [logger] Start file log (type: full)
INFO  [logger] [logger] ./log/2024-01-11/07_15_10.ulg   
INFO  [logger] Opened full log file: ./log/2024-01-11/07_15_10.ulg
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [px4] Startup script returned successfully
INFO: Takeoff Operation started.
INFO  [mavlink] partner IP: 127.0.0.1
pxh> INFO  [commander] Ready for takeoff!
pxh> COMMAND_LONG dump:
Header:
pxh>   Message Type: COMMAND_LONG
  System ID: 255
  Component ID: 190
  Command: 22
  Param1: -1.0
pxh>   Param2: 0.0
  Param3: 0.0
  Param4: nan
  Param5: nan
  Param6: nan
  Param7: 131.321
  Target System: 1
  Target Component: 1
  Confirmation: 0
pxh> INFO: takeoff event start_takeoff
pxh> PX4: 4030564:COMMAND_ACK
pxh> COMMAND_LONG dump:
pxh> Header:
  Message Type: COMMAND_LONG
pxh>   System ID: 255
  Component ID: 190
  Command: 400
pxh>   Param1: 1.0
  Param2: 0.0
  Param3: 0.0
  Param4: 0.0
pxh>   Param5: 0.0
  Param6: 0.0
  Param7: 0.0
pxh>   Target System: 1
  Target Component: 1
  Confirmation: 0
pxh> INFO: takeoff event start_arm
pxh> INFO  [commander] Armed by external command    
pxh> INFO  [tone_alarm] arming warning
pxh> PX4: 5031150:COMMAND_ACK
pxh> INFO  [commander] Takeoff detected 
INFO: Takeoff Operation done.
pxh> INFO: start_landing operation lat: 476414680 lon: -1221405253 alt: 131.321
INFO: StartLanding Operation started.
pxh> COMMAND_INT dump:
Header:
pxh>   Message Type: COMMAND_INT
  System ID: 255
pxh>   Component ID: 190
  Command: 192
pxh>   Param1: 0
pxh>   Param2: 0
pxh>   Param3: 0
  Param4: nan
  X: 476414680
  Y: -1221405253
  Z: 131.321
  Target System: 1
pxh>   Target Component: 1
pxh>   Frame: 0
pxh>   Current: 0
pxh>   Autocontinue: 0
pxh> INFO: landing event start_landing
pxh> INFO  [tone_alarm] notify negative
pxh> PX4: 34144508:COMMAND_ACK
WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
pxh> INFO: StartLanding Operation done.
pxh> INFO: start_landing operation lat: 476418283 lon: -1221401650 alt: 131.321
INFO: StartLanding Operation started.
COMMAND_INT dump:
pxh> Header:
  Message Type: COMMAND_INT
pxh>   System ID: 255
pxh>   Component ID: 190
  Command: 192
  Param1: 0
  Param2: 0
  Param3: 0
pxh>   Param4: nan
  X: 476418283
  Y: -1221401650
pxh>   Z: 131.321
pxh>   Target System: 1
pxh>   Target Component: 1
pxh>   Frame: 0
pxh>   Current: 0
pxh>   Autocontinue: 0
pxh> INFO: landing event start_landing
INFO  [tone_alarm] notify negative
PX4: 93384880:COMMAND_ACK
pxh> WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
pxh> INFO: StartLanding Operation done.
INFO: start_landing operation lat: 476414680 lon: -1221401650 alt: 131.321
INFO: StartLanding Operation started.
pxh> COMMAND_INT dump:
Header:
  Message Type: COMMAND_INT
  System ID: 255
  Component ID: 190
  Command: 192
pxh>   Param1: 0
  Param2: 0
  Param3: 0
  Param4: nan
  X: 476414680
  Y: -1221401650
pxh>   Z: 131.321
  Target System: 1
pxh>   Target Component: 1
pxh>   Frame: 0
pxh>   Current: 0
  Autocontinue: 0
pxh> INFO: landing event start_landing
PX4: 152651632:COMMAND_ACK
pxh> INFO  [tone_alarm] notify negative
pxh> INFO: StartLanding Operation done.
pxh> INFO: start_landing operation lat: 476418283 lon: -1221401650 alt: 131.321
INFO: StartLanding Operation started.
COMMAND_INT dump:
Header:
pxh>   Message Type: COMMAND_INT
  System ID: 255
  Component ID: 190
  Command: 192
  Param1: 0
pxh>   Param2: 0
  Param3: 0
pxh>   Param4: nan
pxh>   X: 476418283
pxh>   Y: -1221401650
pxh>   Z: 131.321
pxh>   Target System: 1
  Target Component: 1
pxh>   Frame: 0
pxh>   Current: 0
pxh>   Autocontinue: 0
INFO: landing event start_landing
pxh> PX4: 211890590:COMMAND_ACK
pxh> INFO  [tone_alarm] notify negative
pxh> WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
WARN  [health_and_arming_checks] Compass needs calibration - Land now!  
WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
WARN  [health_and_arming_checks] Compass needs calibration - Land now!  
pxh> INFO: StartLanding Operation done.
pxh> INFO: All Operations done.
pxh> INFO: Program is exiting.
INFO: stop simulation
pxh> INFO: Stopping processes...
pxh> kill -TERM 66700
kill -TERM 66702
pxh> make: *** [px4_sitl] Killed: 9
FAILED: src/modules/simulation/simulator_mavlink/CMakeFiles/none_iris /Users/tmori/project/oss/hakoniwa-px4sim/px4/PX4-Autopilot/build/px4_sitl_default/src/modules/simulation/simulator_mavlink/CMakeFiles/none_iris 
cd /Users/tmori/project/oss/hakoniwa-px4sim/px4/PX4-Autopilot/build/px4_sitl_default/src/modules/simulation/simulator_mavlink && /opt/homebrew/Cellar/cmake/3.27.7/bin/cmake -E env PX4_SYS_AUTOSTART=10016 /Users/tmori/project/oss/hakoniwa-px4sim/px4/PX4-Autopilot/build/px4_sitl_default/bin/px4
ninja: build stopped: subcommand failed.
INFO: All processes stopped.
relative_start_time: 209.281
relative_end_time: 214.281
actual_mean: -10.047651871025794
actual_stdev: 0.030107103526878936
mean_score: 89.26729160570093
upper_score: 98.47268919322288
lower_score: 73.90993666042374
total_score: 87.21663915311585
Score: 87.21663915311585
relative_start_time: 209.281
relative_end_time: 214.281
actual_mean: 40.4612796574685
actual_stdev: 0.35806514298238784
mean_score: 0.0023964281195936682
upper_score: 2.6444981014907484e-13
lower_score: 58.70403042893132
total_score: 19.56880895235039
Score: 19.56880895235039
relative_start_time: 209.281
relative_end_time: 214.281
actual_mean: -0.0017424037192561783
actual_stdev: 0.36738110014568515
mean_score: 99.98482129847709
upper_score: 0.12500361398311918
lower_score: 0.10998207628956566
total_score: 33.40660232958326
Score: 33.40660232958326
INFO: END TEST

kenjihiranabe commented 10 months ago

今までの修正では結果変わらないと思う。名前直してコンパイル通ったら、ぼくの方でも考えないといけないとろがあるはず。

tmori commented 10 months ago

やっぱり、そうでしたか。地上座標系の修正施して、動かしてみましたが、不安定になってしまいましたので。。

tmori commented 10 months ago

再再チャレンジ

コンフィグ：

{
  "simulation": {
    "lockstep": true,
    "timeStep": 0.003,
    "logOutputDirectory": "/Users/tmori/project/oss/hakoniwa-px4sim/px4/auto-test/test_resource/logs/",
    "logOutput": {
      "sensors": {
          "acc": true,
          "gyro": true,
          "mag": true,
          "baro": true,
          "gps": true    
      },
      "mavlink": {
          "hil_sensor": true,
          "hil_gps": true,
          "hil_actuator_controls": true
      }
    },
    "mavlink_tx_period_msec": {
      "hil_sensor": 3,
      "hil_gps": 3
    },
    "location": {
      "latitude": 47.641468,
      "longitude": -122.140165,
      "altitude": 121.321,
      "magneticField": {
        "intensity_nT": 53045.1,
        "declination_deg": 15.306,
        "inclination_deg": 68.984
      }  
    }
  },
  "components": {
      "droneDynamics": {
          "physicsEquation": "BodyFrame",
          "collision_detection": false,
          "airFrictionCoefficient": 0.0001,
          "inertia": [ 0.0000625, 0.00003125, 0.00009375 ],
          "mass_kg": 0.1,
          "body_size": [ 0.1, 0.1, 0.01 ],
          "position_meter": [ 0, 0, 0 ],
          "angle_degree": [ 0, 0, 90 ]
        },
        "rotor": {
          "vendor": "None",
          "Tr": 0.1,
          "Kr": 6000,
          "rpmMax": 8000
        },
        "thruster": {
          "vendor": "None",
          "rotorPositions": [
            {"position": [  0.05,  0.05, 0 ], "rotationDirection":  1.0 },
            {"position": [ -0.05, -0.05, 0 ], "rotationDirection":  1.0 },
            {"position": [  0.05, -0.05, 0 ], "rotationDirection": -1.0 },
            {"position": [ -0.05,  0.05, 0 ], "rotationDirection": -1.0 }
            ],
          "HoveringRpm": 4000,
          "parameterB":  30.0e-10,
          "parameterJr": 3000.0e-10
        },
        "sensors": {
          "acc": {
            "sampleCount": 1,
            "noise": 0.03
          },
          "gyro": {
            "sampleCount": 1,
            "noise": 0.01
          },
          "mag": {
            "sampleCount": 1,
            "noise": 0.03
          },
          "baro": {
            "sampleCount": 1,
            "noise": 0.01
          },
          "gps": {
            "sampleCount": 1,
            "noise": 0
          }
        }
  }
}

tmori commented 10 months ago

機体座標系

drone_dynamics.csv

tmori@TakashinoMacBook-Pro hakoniwa-px4sim % bash px4/auto-test/auto-test.bash ./px4/auto-test/test_resource px4/auto-test/test_scenario/takeoff_and_move.json 
INFO: START TEST PARAM=6000-0.1
INFO: Activating hakoniwa
HAKO_CAPTURE_SAVE_FILEPATH : ./capture.bin
HAKO_BYPASS_IPADDR : 127.0.0.1
HAKO_CUSTOM_JSON_PATH : ../config/custom.json
DRONE_CONFIG_PATH : /Users/tmori/project/oss/hakoniwa-px4sim/./px4/auto-test/test_resource/work/drone_config.json
HAKO_BYPASS_PORTNO : 54001
INFO: shmget() key=255 size=1129352 
INFO: hako_master_init() success
Robot: DroneAvator, PduWriter: DroneAvator_drone_motor
channel_id: 0 pdu_size: 88
INFO: DroneAvator create_lchannel: logical_id=0 real_id=0 size=88
Robot: DroneAvator, PduWriter: DroneAvator_drone_pos
channel_id: 1 pdu_size: 48
INFO: DroneAvator create_lchannel: logical_id=1 real_id=1 size=48
WAIT START
INFO: Activating PX4
ps: stdin: not a terminal
[0/1] launching px4 none_iris (SYS_AUTOSTART=10016)

______  __   __    ___ 
| ___ \ \ \ / /   /   |
| |_/ /  \ V /   / /| |
|  __/   /   \  / /_| |
| |     / /^\ \ \___  |
\_|     \/   \/     |_/

px4 starting.

INFO  [px4] startup script: /bin/sh etc/init.d-posix/rcS 0
env SYS_AUTOSTART: 10016
INFO  [param] selected parameter default file parameters.bson
INFO  [param] importing from 'parameters.bson'
INFO  [parameters] BSON document size 971 bytes, decoded 971 bytes (INT32:15, FLOAT:29)
INFO  [param] selected parameter backup file parameters_backup.bson
INFO  [dataman] data manager file './dataman' size is 7868392 bytes
INFO  [init] PX4_SIM_HOSTNAME: localhost
INFO  [simulator_mavlink] Waiting for simulator to accept connection on TCP port 4560
INFO  [simulator_mavlink] Simulator connected on TCP port 4560.
INFO: px4 reciver start
INFO: COMMAND_LONG ack sended
ERROR [simulator_mavlink] poll timeout 0, 22
ERROR [simulator_mavlink] poll timeout 0, 22
INFO: start simulation
start
WAIT RUNNING
START CREATE PDU DATA: total_size= 136
INFO: shmget() key=256 size=136 
PDU DATA CREATED
CREATED ADDR=0x10055800c
PDU CREATED
INFO: setup start
Rotor vendor: None
Thruster vendor: None
param_A: 1.53281e-08
param_B: 3e-09
param_Jr: 3e-07
INFO: setup done
INFO: start simulation
SYNC MODE: true
Robot: DroneAvator, PduWriter: DroneAvator_drone_motor
channel_id: 0 pdu_size: 88
Robot: DroneAvator, PduWriter: DroneAvator_drone_pos
channel_id: 1 pdu_size: 48
INFO  [lockstep_scheduler] setting initial absolute time to 1705135773780281 us
WARN  [vehicle_angular_velocity] no gyro selected, using sensor_gyro_fifo:0 1310988
INFO  [commander] LED: open /dev/led0 failed (22)
WARN  [health_and_arming_checks] Preflight Fail: ekf2 missing data
INFO  [tone_alarm] home set
WARN  [health_and_arming_checks] Preflight Fail: ekf2 missing data
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550
me:192.168.11.33:54001
px4:127.0.0.1:18570
qgc:192.168.11.56:14550
INFO: takeoff operation alt: 131.321
INFO  [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540
INFO  [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14280 remote port 14030
INFO  [mavlink] mode: Gimbal, data rate: 400000 B/s on udp port 13030 remote port 13280
INFO  [logger] logger started (mode=all)
INFO  [logger] Start file log (type: full)
INFO  [logger] [logger] ./log/2024-01-13/08_49_51.ulg   
INFO  [logger] Opened full log file: ./log/2024-01-13/08_49_51.ulg
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [px4] Startup script returned successfully
INFO: Takeoff Operation started.
INFO  [commander] Ready for takeoff!
pxh> INFO  [mavlink] partner IP: 127.0.0.1
pxh> COMMAND_LONG dump:
Header:
pxh>   Message Type: COMMAND_LONG
pxh>   System ID: 255
  Component ID: 190
  Command: 22
  Param1: -1.0
  Param2: 0.0
pxh>   Param3: 0.0
pxh>   Param4: nan
  Param5: nan
  Param6: nan
  Param7: 131.321
  Target System: 1
  Target Component: 1
pxh>   Confirmation: 0
INFO: takeoff event start_takeoff
pxh> PX4: 4042444:COMMAND_ACK
pxh> COMMAND_LONG dump:
Header:
  Message Type: COMMAND_LONG
pxh>   System ID: 255
pxh>   Component ID: 190
pxh>   Command: 400
pxh>   Param1: 1.0
pxh>   Param2: 0.0
  Param3: 0.0
pxh>   Param4: 0.0
pxh>   Param5: 0.0
pxh>   Param6: 0.0
  Param7: 0.0
  Target System: 1
pxh>   Target Component: 1
  Confirmation: 0
pxh> INFO: takeoff event start_arm
pxh> INFO  [commander] Armed by external command    
pxh> INFO  [tone_alarm] arming warning
PX4: 5033403:COMMAND_ACK
pxh> INFO  [commander] Takeoff detected 
pxh> INFO: Takeoff Operation done.
pxh> INFO: start_landing operation lat: 476414680 lon: -1221405253 alt: 131.321
INFO: StartLanding Operation started.
pxh> COMMAND_INT dump:
pxh> Header:
  Message Type: COMMAND_INT
pxh>   System ID: 255
  Component ID: 190
  Command: 192
pxh>   Param1: 0
pxh>   Param2: 0
  Param3: 0
  Param4: nan
  X: 476414680
  Y: -1221405253
pxh>   Z: 131.321
  Target System: 1
  Target Component: 1
pxh>   Frame: 0
pxh>   Current: 0
  Autocontinue: 0
INFO: landing event start_landing
pxh> INFO  [tone_alarm] notify negative
PX4: 34156798:COMMAND_ACK
pxh> INFO: StartLanding Operation done.
pxh> INFO: start_landing operation lat: 476418283 lon: -1221401650 alt: 131.321
pxh> INFO: StartLanding Operation started.
COMMAND_INT dump:
pxh> Header:
  Message Type: COMMAND_INT
pxh>   System ID: 255
  Component ID: 190
  Command: 192
pxh>   Param1: 0
pxh>   Param2: 0
pxh>   Param3: 0
pxh>   Param4: nan
  X: 476418283
  Y: -1221401650
  Z: 131.321
pxh>   Target System: 1
pxh>   Target Component: 1
pxh>   Frame: 0
pxh>   Current: 0
pxh>   Autocontinue: 0
INFO: landing event start_landing
pxh> INFO  [tone_alarm] notify negative
PX4: 93408577:COMMAND_ACK
pxh> WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
pxh> WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
pxh> INFO: StartLanding Operation done.
pxh> INFO: start_landing operation lat: 476414680 lon: -1221401650 alt: 131.321
pxh> INFO: StartLanding Operation started.
COMMAND_INT dump:
Header:
  Message Type: COMMAND_INT
  System ID: 255
  Component ID: 190
  Command: 192
  Param1: 0
  Param2: 0
  Param3: 0
  Param4: nan
  X: 476414680
  Y: -1221401650
  Z: 131.321
  Target System: 1
  Target Component: 1
  Frame: 0
  Current: 0
  Autocontinue: 0
pxh> INFO: landing event start_landing
pxh> INFO  [tone_alarm] notify negative
PX4: 152665821:COMMAND_ACK
INFO: StartLanding Operation done.
pxh> INFO: start_landing operation lat: 476418283 lon: -1221401650 alt: 131.321
INFO: StartLanding Operation started.
pxh> COMMAND_INT dump:
Header:
pxh>   Message Type: COMMAND_INT
  System ID: 255
pxh>   Component ID: 190
pxh>   Command: 192
pxh>   Param1: 0
  Param2: 0
  Param3: 0
pxh>   Param4: nan
pxh>   X: 476418283
pxh>   Y: -1221401650
pxh>   Z: 131.321
pxh>   Target System: 1
  Target Component: 1
  Frame: 0
  Current: 0
  Autocontinue: 0
pxh> INFO: landing event start_landing
pxh> INFO  [tone_alarm] notify negative
pxh> PX4: 211907693:COMMAND_ACK
pxh> WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
pxh> WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
INFO: StartLanding Operation done.
pxh>INFO: All Operations done.
INFO: Program is exiting.
INFO: stop simulation
pxh> INFO: Stopping processes...
kill -TERM 18848
pxh> kill -TERM 18850
make: *** [px4_sitl] Killed: 9
INFO: All processes stopped.
FAILED: src/modules/simulation/simulator_mavlink/CMakeFiles/none_iris /Users/tmori/project/oss/hakoniwa-px4sim/px4/PX4-Autopilot/build/px4_sitl_default/src/modules/simulation/simulator_mavlink/CMakeFiles/none_iris 
cd /Users/tmori/project/oss/hakoniwa-px4sim/px4/PX4-Autopilot/build/px4_sitl_default/src/modules/simulation/simulator_mavlink && /opt/homebrew/Cellar/cmake/3.27.7/bin/cmake -E env PX4_SYS_AUTOSTART=10016 /Users/tmori/project/oss/hakoniwa-px4sim/px4/PX4-Autopilot/build/px4_sitl_default/bin/px4
ninja: build stopped: subcommand failed.
relative_start_time: 209.962
relative_end_time: 214.962
actual_mean: -9.988181611877625
actual_stdev: 0.008002404588227981
mean_score: 99.30406145766483
upper_score: 98.05484792381952
lower_score: 99.92721784740276
total_score: 99.09537574296236
Score: 99.09537574296236
relative_start_time: 209.962
relative_end_time: 214.962
actual_mean: 39.87888619436113
actual_stdev: 0.06840234152663892
mean_score: 48.02599804272266
upper_score: 87.02933655055332
lower_score: 16.599153235720184
total_score: 50.55149594299872
Score: 50.55149594299872
relative_start_time: 209.962
relative_end_time: 214.962
actual_mean: -0.059625599880024006
actual_stdev: 0.10648644766634087
mean_score: 83.71428047492317
upper_score: 89.60160504109085
lower_score: 25.16639490334047
total_score: 66.16076013978483
Score: 66.16076013978483
INFO: END TEST

tmori commented 10 months ago

地上座標系

drone_dynamics.csv

tmori@TakashinoMacBook-Pro hakoniwa-px4sim % bash px4/auto-test/auto-test.bash ./px4/auto-test/test_resource px4/auto-test/test_scenario/takeoff_and_move.json
INFO: START TEST PARAM=6000-0.1
INFO: Activating hakoniwa
HAKO_CAPTURE_SAVE_FILEPATH : ./capture.bin
HAKO_BYPASS_IPADDR : 127.0.0.1
HAKO_CUSTOM_JSON_PATH : ../config/custom.json
DRONE_CONFIG_PATH : /Users/tmori/project/oss/hakoniwa-px4sim/./px4/auto-test/test_resource/work/drone_config.json
HAKO_BYPASS_PORTNO : 54001
INFO: shmget() key=255 size=1129352 
INFO: hako_master_init() success
Robot: DroneAvator, PduWriter: DroneAvator_drone_motor
channel_id: 0 pdu_size: 88
INFO: DroneAvator create_lchannel: logical_id=0 real_id=0 size=88
Robot: DroneAvator, PduWriter: DroneAvator_drone_pos
channel_id: 1 pdu_size: 48
INFO: DroneAvator create_lchannel: logical_id=1 real_id=1 size=48
WAIT START
INFO: Activating PX4
ps: stdin: not a terminal
[0/1] launching px4 none_iris (SYS_AUTOSTART=10016)

______  __   __    ___ 
| ___ \ \ \ / /   /   |
| |_/ /  \ V /   / /| |
|  __/   /   \  / /_| |
| |     / /^\ \ \___  |
\_|     \/   \/     |_/

px4 starting.

INFO  [px4] startup script: /bin/sh etc/init.d-posix/rcS 0
env SYS_AUTOSTART: 10016
INFO  [param] selected parameter default file parameters.bson
INFO  [param] importing from 'parameters.bson'
INFO  [parameters] BSON document size 971 bytes, decoded 971 bytes (INT32:15, FLOAT:29)
INFO  [param] selected parameter backup file parameters_backup.bson
INFO  [dataman] data manager file './dataman' size is 7868392 bytes
INFO  [init] PX4_SIM_HOSTNAME: localhost
INFO  [simulator_mavlink] Waiting for simulator to accept connection on TCP port 4560
INFO  [simulator_mavlink] Simulator connected on TCP port 4560.
INFO: px4 reciver start
INFO: COMMAND_LONG ack sended
ERROR [simulator_mavlink] poll timeout 0, 22
ERROR [simulator_mavlink] poll timeout 0, 22
INFO: start simulation
start
WAIT RUNNING
PDU CREATED
START CREATE PDU DATA: total_size= 136
INFO: shmget() key=256 size=136 
PDU DATA CREATED
CREATED ADDR=0x104a8c00c
INFO: setup start
Rotor vendor: None
Thruster vendor: None
param_A: 1.53281e-08
param_B: 3e-09
param_Jr: 3e-07
INFO: setup done
INFO: start simulation
SYNC MODE: true
Robot: DroneAvator, PduWriter: DroneAvator_drone_motor
channel_id: 0 pdu_size: 88
Robot: DroneAvator, PduWriter: DroneAvator_drone_pos
channel_id: 1 pdu_size: 48
INFO  [lockstep_scheduler] setting initial absolute time to 1705136239789794 us
WARN  [vehicle_angular_velocity] no gyro selected, using sensor_gyro_fifo:0 1310988
INFO  [commander] LED: open /dev/led0 failed (22)
WARN  [health_and_arming_checks] Preflight Fail: ekf2 missing data
INFO  [tone_alarm] home set
WARN  [health_and_arming_checks] Preflight Fail: ekf2 missing data
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550
me:192.168.11.33:54001
px4:127.0.0.1:18570
qgc:192.168.11.56:14550
INFO: takeoff operation alt: 131.321
INFO  [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540
INFO  [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14280 remote port 14030
INFO  [mavlink] mode: Gimbal, data rate: 400000 B/s on udp port 13030 remote port 13280
INFO  [logger] logger started (mode=all)
INFO  [logger] Start file log (type: full)
INFO  [logger] [logger] ./log/2024-01-13/08_57_36.ulg   
INFO  [logger] Opened full log file: ./log/2024-01-13/08_57_36.ulg
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [px4] Startup script returned successfully
INFO: Takeoff Operation started.
pxh> INFO  [commander] Ready for takeoff!
pxh> INFO  [mavlink] partner IP: 127.0.0.1
COMMAND_LONG dump:
pxh> Header:
  Message Type: COMMAND_LONG
pxh>   System ID: 255
pxh>   Component ID: 190
  Command: 22
pxh>   Param1: -1.0
pxh>   Param2: 0.0
pxh>   Param3: 0.0
  Param4: nan
pxh>   Param5: nan
  Param6: nan
pxh>   Param7: 131.321
pxh>   Target System: 1
pxh>   Target Component: 1
  Confirmation: 0
pxh> INFO: takeoff event start_takeoff
PX4: 4036788:COMMAND_ACK
COMMAND_LONG dump:
pxh> Header:
  Message Type: COMMAND_LONG
  System ID: 255
  Component ID: 190
  Command: 400
  Param1: 1.0
pxh>   Param2: 0.0
pxh>   Param3: 0.0
pxh>   Param4: 0.0
pxh>   Param5: 0.0
  Param6: 0.0
  Param7: 0.0
  Target System: 1
pxh>   Target Component: 1
pxh>   Confirmation: 0
pxh> INFO: takeoff event start_arm
INFO  [commander] Armed by external command 
pxh> INFO  [tone_alarm] arming warning
pxh> PX4: 5035410:COMMAND_ACK
pxh> INFO  [commander] Takeoff detected 
pxh> INFO: Takeoff Operation done.
pxh> INFO: start_landing operation lat: 476414680 lon: -1221405253 alt: 131.321
INFO: StartLanding Operation started.
COMMAND_INT dump:
Header:
  Message Type: COMMAND_INT
pxh>   System ID: 255
  Component ID: 190
  Command: 192
  Param1: 0
pxh>   Param2: 0
pxh>   Param3: 0
pxh>   Param4: nan
  X: 476414680
pxh>   Y: -1221405253
  Z: 131.321
  Target System: 1
pxh>   Target Component: 1
  Frame: 0
pxh>   Current: 0
pxh>   Autocontinue: 0
pxh> INFO: landing event start_landing
pxh> INFO  [tone_alarm] notify negative
pxh> PX4: 34153162:COMMAND_ACK
pxh> INFO: StartLanding Operation done.
INFO: start_landing operation lat: 476418283 lon: -1221401650 alt: 131.321
INFO: StartLanding Operation started.
pxh> COMMAND_INT dump:
Header:
pxh>   Message Type: COMMAND_INT
  System ID: 255
  Component ID: 190
  Command: 192
  Param1: 0
pxh>   Param2: 0
pxh>   Param3: 0
pxh>   Param4: nan
pxh>   X: 476418283
  Y: -1221401650
pxh>   Z: 131.321
  Target System: 1
  Target Component: 1
pxh>   Frame: 0
  Current: 0
  Autocontinue: 0
pxh> INFO: landing event start_landing
pxh> INFO  [tone_alarm] notify negative
pxh>PX4: 93404407:COMMAND_ACK
pxh> WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
WARN  [health_and_arming_checks] Preflight Fail: High Accelerometer Bias
pxh> INFO: StartLanding Operation done.
INFO: start_landing operation lat: 476414680 lon: -1221401650 alt: 131.321
INFO: StartLanding Operation started.
COMMAND_INT dump:
Header:
  Message Type: COMMAND_INT
  System ID: 255
  Component ID: 190
  Command: 192
  Param1: 0
  Param2: 0
  Param3: 0
pxh>   Param4: nan
pxh>   X: 476414680
  Y: -1221401650
pxh>   Z: 131.321
  Target System: 1
  Target Component: 1
  Frame: 0
  Current: 0
  Autocontinue: 0
INFO: landing event start_landing
pxh> INFO  [tone_alarm] notify negative
PX4: 152624140:COMMAND_ACK
pxh> INFO: StartLanding Operation done.
INFO: start_landing operation lat: 476418283 lon: -1221401650 alt: 131.321
pxh> INFO: StartLanding Operation started.
COMMAND_INT dump:
Header:
  Message Type: COMMAND_INT
  System ID: 255
  Component ID: 190
  Command: 192
pxh>   Param1: 0
  Param2: 0
pxh>   Param3: 0
  Param4: nan
  X: 476418283
  Y: -1221401650
  Z: 131.321
pxh>   Target System: 1
  Target Component: 1
  Frame: 0
  Current: 0
pxh>   Autocontinue: 0
INFO: landing event start_landing
pxh> INFO  [tone_alarm] notify negative
pxh> PX4: 211887514:COMMAND_ACK
pxh> INFO: StartLanding Operation done.
INFO: All Operations done.
INFO: Program is exiting.
pxh> INFO: stop simulation
INFO: Stopping processes...
kill -TERM 19725
pxh> kill -TERM 19727
make: *** [px4_sitl] Killed: 9
INFO: All processes stopped.
FAILED: src/modules/simulation/simulator_mavlink/CMakeFiles/none_iris /Users/tmori/project/oss/hakoniwa-px4sim/px4/PX4-Autopilot/build/px4_sitl_default/src/modules/simulation/simulator_mavlink/CMakeFiles/none_iris 
cd /Users/tmori/project/oss/hakoniwa-px4sim/px4/PX4-Autopilot/build/px4_sitl_default/src/modules/simulation/simulator_mavlink && /opt/homebrew/Cellar/cmake/3.27.7/bin/cmake -E env PX4_SYS_AUTOSTART=10016 /Users/tmori/project/oss/hakoniwa-px4sim/px4/PX4-Autopilot/build/px4_sitl_default/bin/px4
ninja: build stopped: subcommand failed.
relative_start_time: 209.953
relative_end_time: 214.953
actual_mean: -9.986090503899218
actual_stdev: 0.006823224802004907
mean_score: 99.03729356722738
upper_score: 97.87370276115962
lower_score: 99.74923872807445
total_score: 98.88674501882048
Score: 98.88674501882048
relative_start_time: 209.953
relative_end_time: 214.953
actual_mean: 39.867474435512904
actual_stdev: 0.09103747507455108
mean_score: 41.555044582385634
upper_score: 91.75363528942987
lower_score: 8.216534718456664
total_score: 47.175071530090726
Score: 47.175071530090726
relative_start_time: 209.953
relative_end_time: 214.953
actual_mean: -0.06085486502699459
actual_stdev: 0.0940511692461942
mean_score: 83.09665564580607
upper_score: 94.63907590427716
lower_score: 30.125627253161934
total_score: 69.28711960108173
Score: 69.28711960108173
INFO: END TEST

tmori commented 10 months ago

結論

同じ結果に見えますね。compassエラーもない。

kenjihiranabe commented 10 months ago

Yeah! 手計算で微分してみました。

kenjihiranabe commented 10 months ago

残すはドキュメントの変更のみ。

tmori commented 8 months ago

地上座標系の対応は以下で実施しているのためクローズします。

https://github.com/toppers/hakoniwa-px4sim/blob/main/hakoniwa/src/assets/drone/physics/ground_frame/drone_dynamics_ground_frame.hpp