Skip to content

Commit 18d76e3

Browse files
jonas-eschmannjonas-eschmann
committed
more debug but no success
1 parent 5c0a6d9 commit 18d76e3

5 files changed

Lines changed: 69 additions & 34 deletions

File tree

‎boards/px4/fmu-v6c/raptor.px4board‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -44,11 +44,11 @@ CONFIG_MODULES_EKF2=y
4444
CONFIG_MODULES_ESC_BATTERY=y
4545
CONFIG_MODULES_EVENTS=y
4646
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
47-
CONFIG_MODULES_FW_ATT_CONTROL=y
48-
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
49-
CONFIG_MODULES_FW_MODE_MANAGER=y
50-
CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
51-
CONFIG_MODULES_FW_RATE_CONTROL=y
47+
CONFIG_MODULES_FW_ATT_CONTROL=n
48+
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
49+
CONFIG_MODULES_FW_MODE_MANAGER=n
50+
CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
51+
CONFIG_MODULES_FW_RATE_CONTROL=n
5252
CONFIG_MODULES_GIMBAL=y
5353
CONFIG_MODULES_GYRO_CALIBRATION=y
5454
CONFIG_MODULES_LAND_DETECTOR=y
@@ -70,7 +70,7 @@ CONFIG_MODULES_SENSORS=y
7070
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
7171
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
7272
CONFIG_MODULES_UXRCE_DDS_CLIENT=y
73-
CONFIG_MODULES_VTOL_ATT_CONTROL=y
73+
CONFIG_MODULES_VTOL_ATT_CONTROL=n
7474
CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
7575
CONFIG_SYSTEMCMDS_BSONDUMP=y
7676
CONFIG_SYSTEMCMDS_DMESG=y

‎msg/versioned/RaptorStatus.msg‎

Lines changed: 8 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -3,9 +3,8 @@ uint64 timestamp
33
uint64 timestamp_sample
44
bool subscription_update_angular_velocity
55
bool subscription_update_local_position
6-
bool subscription_update_visual_odometry
76
bool subscription_update_attitude
8-
bool subscription_update_rl_tools_command
7+
bool subscription_update_trajectory_setpoint
98
bool subscription_update_vehicle_status
109

1110
uint8 exit_reason
@@ -20,20 +19,16 @@ uint8 EXIT_REASON_VISUAL_ODOMETRY_NAN = 7
2019
uint8 EXIT_REASON_ATTITUDE_STALE = 8
2120
uint8 EXIT_REASON_EXECUTOR_STATUS_SOURCE_NOT_CONTROL = 9
2221

23-
uint8 ODOMETRY_SOURCE_LOCAL_POSITION = 0
24-
uint8 ODOMETRY_SOURCE_VISUAL_ODOMETRY = 1
25-
uint8 ODOMETRY_SOURCE_UNKNOWN = 2
22+
uint32 timestamp_last_vehicle_angular_velocity
23+
uint32 timestamp_last_vehicle_local_position
24+
uint32 timestamp_last_vehicle_attitude
25+
uint32 timestamp_last_trajectory_setpoint
2626

27+
bool vehicle_angular_velocity_stale
28+
bool vehicle_local_position_stale
29+
bool vehicle_attitude_stale
2730
bool trajectory_setpoint_stale
2831

2932
bool active
3033
uint8 substep
31-
uint8 odometry_source
3234
float32 control_interval
33-
34-
uint32 visual_odometry_stale_counter
35-
uint64 visual_odometry_age
36-
float32 visual_odometry_dt_mean
37-
float32 visual_odometry_dt_std
38-
uint8 NUM_VISUAL_ODOMETRY_DT_MAX = 3
39-
float32[3] visual_odometry_dt_max

‎src/modules/mc_raptor/README.md‎

Lines changed: 22 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,8 @@
11
# RAPTOR
22

33

4-
## Standalone Usage (Without External Trajectory Setpoint)
4+
## SITL
5+
#### Standalone Usage (Without External Trajectory Setpoint)
56
Build PX4 SITL with Raptor, disable QGC requirement, and adjust the `IMU_GYRO_RATEMAX` to match the simulation IMU rate
67
```
78
make px4_sitl_raptor gz_x500
@@ -31,7 +32,7 @@ commander mode ext{RAPTOR_MODE_ID}
3132
```
3233

3334

34-
## Usage with External Trajectory Setpoint
35+
#### Usage with External Trajectory Setpoint
3536

3637

3738
Send Lissajous setpoints via Mavlink:
@@ -40,3 +41,22 @@ pip install px4
4041
px4 udp:localhost:14540 track lissajous --A 2.0 --B 0.5 --duration 5 --ramp-duration 3 --takeoff 3.0
4142
```
4243

44+
45+
## SIH
46+
```
47+
make px4_fmu-v6c_raptor upload
48+
```
49+
In QGroundControl:
50+
- Airframes => SIH Quadrotor X
51+
- Settings => Comm Links => Disable Pixhawk (disable automatic USB serial connection)
52+
```
53+
mavproxy.py --master /dev/serial/by-id/usb-Auterion_PX4_FMU_v6C.x_0-if00 --out udp:localhost:14550 --out udp:localhost:13337 --out udp:localhost:13338
54+
```
55+
New terminal:
56+
```
57+
./Tools/simulation/jmavsim/jmavsim_run.sh -u -p 13337 -o
58+
```
59+
New terminal (optional):
60+
```
61+
./Tools/mavlink_shell.py udp:localhost:13338
62+
```

‎src/modules/mc_raptor/mc_raptor.cpp‎

Lines changed: 31 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@ Raptor::Raptor(): ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_
2020
last_native_status_set = false;
2121
policy_frequency_check_counter = 0;
2222
flightmode_state = FlightModeState::UNREGISTERED;
23+
can_arm = false;
2324

2425
_actuator_motors_pub.advertise();
2526
_tune_control_pub.advertise();
@@ -352,7 +353,7 @@ void Raptor::observe(rl_tools::inference::applications::l2f::Observation<EXECUTO
352353
}
353354

354355

355-
void Raptor::updateArmingCheckReply(bool active){
356+
void Raptor::updateArmingCheckReply(){
356357
if(flightmode_state == FlightModeState::CONFIGURED){
357358
if (_arming_check_request_sub.updated()) {
358359
arming_check_request_s arming_check_request;
@@ -363,7 +364,7 @@ void Raptor::updateArmingCheckReply(bool active){
363364
arming_check_reply.registration_id = ext_component_arming_check_id;
364365
arming_check_reply.health_component_index = arming_check_reply.HEALTH_COMPONENT_INDEX_NONE;
365366
arming_check_reply.num_events = 0;
366-
arming_check_reply.can_arm_and_run = active;
367+
arming_check_reply.can_arm_and_run = can_arm;
367368
arming_check_reply.mode_req_angular_velocity = true;
368369
arming_check_reply.mode_req_local_position = true;
369370
arming_check_reply.mode_req_attitude = true;
@@ -428,7 +429,7 @@ void Raptor::Run(){
428429
perf_begin(_loop_perf);
429430
hrt_abstime current_time = hrt_absolute_time();
430431

431-
raptor_status_s status;
432+
raptor_status_s status{};
432433
status.timestamp = current_time;
433434
status.timestamp_sample = current_time;
434435
status.exit_reason = raptor_status_s::EXIT_REASON_NONE;
@@ -440,6 +441,7 @@ void Raptor::Run(){
440441
status.subscription_update_angular_velocity = _vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity);
441442
if(status.subscription_update_angular_velocity){
442443
timestamp_last_angular_velocity = current_time;
444+
status.timestamp_last_vehicle_angular_velocity = current_time;
443445
timestamp_last_angular_velocity_set = true;
444446
angular_velocity_update = true;
445447
}
@@ -448,17 +450,20 @@ void Raptor::Run(){
448450
status.subscription_update_local_position = _vehicle_local_position_sub.update(&_vehicle_local_position);
449451
if(status.subscription_update_local_position){
450452
timestamp_last_local_position = current_time;
453+
status.timestamp_last_vehicle_local_position = current_time;
451454
timestamp_last_local_position_set = true;
452455
}
453456

454457
status.subscription_update_attitude = _vehicle_attitude_sub.update(&_vehicle_attitude);
455458
if(status.subscription_update_attitude){
456459
timestamp_last_attitude = current_time;
460+
status.timestamp_last_vehicle_attitude = current_time;
457461
timestamp_last_attitude_set = true;
458462
}
459463

460464
trajectory_setpoint_s temp_trajectory_setpoint;
461-
if(_trajectory_setpoint_sub.update(&temp_trajectory_setpoint)) {
465+
status.subscription_update_trajectory_setpoint = _trajectory_setpoint_sub.update(&temp_trajectory_setpoint);
466+
if(status.subscription_update_trajectory_setpoint){
462467
if(
463468
PX4_ISFINITE(temp_trajectory_setpoint.position[0]) &&
464469
PX4_ISFINITE(temp_trajectory_setpoint.position[1]) &&
@@ -470,6 +475,7 @@ void Raptor::Run(){
470475
PX4_ISFINITE(temp_trajectory_setpoint.yawspeed)
471476
){
472477
timestamp_last_trajectory_setpoint_set = true;
478+
status.timestamp_last_trajectory_setpoint = current_time;
473479
timestamp_last_trajectory_setpoint = temp_trajectory_setpoint.timestamp;
474480
_trajectory_setpoint = temp_trajectory_setpoint;
475481
}
@@ -479,16 +485,22 @@ void Raptor::Run(){
479485
if(!angular_velocity_update){
480486
status.exit_reason = raptor_status_s::EXIT_REASON_NO_ANGULAR_VELOCITY_UPDATE;
481487
if constexpr(PUBLISH_NON_COMPLETE_STATUS){
482-
_raptor_status_pub.publish(status);
488+
// _raptor_status_pub.publish(status);
483489
}
490+
updateArmingCheckReply();
484491
return;
485492
}
486493

487494
if(!timestamp_last_angular_velocity_set || !timestamp_last_local_position_set || !timestamp_last_attitude_set){
488495
status.exit_reason = raptor_status_s::EXIT_REASON_NOT_ALL_OBSERVATIONS_SET;
496+
status.vehicle_angular_velocity_stale = !timestamp_last_angular_velocity_set;
497+
status.vehicle_local_position_stale = !timestamp_last_local_position_set;
498+
status.vehicle_attitude_stale = !timestamp_last_attitude_set;
489499
if constexpr(PUBLISH_NON_COMPLETE_STATUS){
490500
_raptor_status_pub.publish(status);
491501
}
502+
can_arm = false;
503+
updateArmingCheckReply();
492504
return;
493505
}
494506

@@ -501,7 +513,8 @@ void Raptor::Run(){
501513
PX4_ERR("angular velocity timeout");
502514
timeout_message_sent = true;
503515
}
504-
updateArmingCheckReply(false);
516+
can_arm = false;
517+
updateArmingCheckReply();
505518
return;
506519
}
507520
if((current_time - timestamp_last_local_position) > OBSERVATION_TIMEOUT_LOCAL_POSITION){
@@ -513,7 +526,8 @@ void Raptor::Run(){
513526
PX4_ERR("local position timeout");
514527
timeout_message_sent = true;
515528
}
516-
updateArmingCheckReply(false);
529+
can_arm = false;
530+
updateArmingCheckReply();
517531
return;
518532
}
519533
else{
@@ -535,11 +549,16 @@ void Raptor::Run(){
535549
PX4_ERR("attitude timeout");
536550
timeout_message_sent = true;
537551
}
538-
updateArmingCheckReply(false);
552+
can_arm = false;
553+
updateArmingCheckReply();
539554
return;
540555
}
541556
timeout_message_sent = false;
542557

558+
// is ready to control at this point
559+
can_arm = true;
560+
updateArmingCheckReply();
561+
543562
if(!timestamp_last_trajectory_setpoint_set || (current_time - timestamp_last_trajectory_setpoint) > TRAJECTORY_SETPOINT_TIMEOUT){
544563
status.trajectory_setpoint_stale = true;
545564
if(!previous_trajectory_setpoint_stale){
@@ -564,6 +583,9 @@ void Raptor::Run(){
564583
}
565584

566585

586+
587+
588+
567589
rl_tools::inference::applications::l2f::Observation<EXECUTOR_SPEC> observation;
568590
rl_tools::inference::applications::l2f::Action<EXECUTOR_SPEC> action;
569591
observe(observation);
@@ -573,7 +595,7 @@ void Raptor::Run(){
573595
if(executor_status.source != decltype(executor_status.source)::CONTROL){
574596
status.exit_reason = raptor_status_s::EXIT_REASON_EXECUTOR_STATUS_SOURCE_NOT_CONTROL;
575597
if constexpr(PUBLISH_NON_COMPLETE_STATUS){
576-
_raptor_status_pub.publish(status);
598+
// _raptor_status_pub.publish(status);
577599
}
578600
return;
579601
}
@@ -592,9 +614,6 @@ void Raptor::Run(){
592614

593615
// no return after this point!
594616

595-
updateArmingCheckReply(true);
596-
597-
598617
raptor_input_s input_msg;
599618
input_msg.active = status.active;
600619
static_assert(raptor_input_s::ACTION_DIM == EXECUTOR_CONFIG::OUTPUT_DIM);

‎src/modules/mc_raptor/mc_raptor.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -123,6 +123,8 @@ class Raptor : public ModuleBase<Raptor>, public ModuleParams, public px4::Sched
123123
CONFIGURED = 2
124124
};
125125
FlightModeState flightmode_state = FlightModeState::UNREGISTERED;
126+
bool can_arm = false;
127+
void updateArmingCheckReply();
126128

127129
// node state
128130
vehicle_local_position_s _vehicle_local_position{};
@@ -247,6 +249,5 @@ class Raptor : public ModuleBase<Raptor>, public ModuleParams, public px4::Sched
247249
(ParamBool<px4::params::MC_RAPTOR_OFFB>) _param_mc_raptor_offboard
248250
)
249251

250-
void updateArmingCheckReply(bool active);
251252

252253
};

0 commit comments

Comments
 (0)