Skip to content

Commit 2d11cc8

Browse files
jonas-eschmannjonas-eschmann
committed
fixing warnings
1 parent 4f3e6ba commit 2d11cc8

3 files changed

Lines changed: 48 additions & 35 deletions

File tree

‎src/lib/rl_tools/CMakeLists.txt‎

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -6,8 +6,9 @@ if(CONFIG_LIB_RL_TOOLS)
66

77
target_compile_features(rl_tools INTERFACE cxx_std_17)
88

9-
# target_compile_options(rl_tools INTERFACE
10-
# -Wno-unused-parameter
11-
# -Wno-unused-variable
12-
# )
9+
target_compile_options(rl_tools INTERFACE
10+
-Wno-unused-parameter
11+
-Wno-unused-variable
12+
-Wno-unused-local-typedefs
13+
)
1314
endif()

‎src/modules/mc_raptor/mc_raptor.cpp‎

Lines changed: 33 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -30,8 +30,6 @@ void Raptor::reset(){
3030
for(TI action_i=0; action_i < EXECUTOR_SPEC::OUTPUT_DIM; action_i++){
3131
this->previous_action[action_i] = RESET_PREVIOUS_ACTION_VALUE;
3232
}
33-
// rl_tools_inference_applications_l2f_reset();
34-
3533
rlt::reset(device, executor, policy, rng);
3634
}
3735

@@ -47,6 +45,7 @@ bool Raptor::test_policy(FILE *f, TI input_offset, TI output_offset){
4745
#endif
4846
using namespace rl_tools::inference::applications::l2f;
4947
#ifndef RL_TOOLS_DISABLE_TEST
48+
// This tests the policy using a known input output pair that has been saved into the policy checkpoint to verify that it has been loaded correctly
5049
EXECUTOR_CONFIG::POLICY_TEST::template Buffer<false> buffers_test;
5150
EXECUTOR_CONFIG::POLICY_TEST::State<false> policy_state_test;
5251
rl_tools::Tensor<rl_tools::tensor::Specification<EXECUTOR_CONFIG::TYPE_POLICY::DEFAULT, TI, rl_tools::tensor::Shape<TI, 1, decltype(policy)::OUTPUT_SHAPE::LAST>, false>> test_output;
@@ -86,20 +85,20 @@ bool Raptor::test_policy(FILE *f, TI input_offset, TI output_offset){
8685
}
8786
}
8887
float abs_diff = acc / num_values;
89-
PX4_INFO("Checkpoint test diff: %f", abs_diff);
88+
PX4_INFO("Checkpoint test diff: %f", (double)abs_diff);
9089
for(TI output_i = 0; output_i < EXECUTOR_CONFIG::OUTPUT_DIM; output_i++){
91-
PX4_INFO("output[%d]: %f", output_i, action.action[output_i]);
90+
PX4_INFO("output[%d]: %f", (int)output_i, (double)action.action[output_i]);
9291
}
9392

9493
constexpr float EPSILON = 1e-5;
9594

9695
bool healthy = abs_diff < EPSILON;
9796
if(!healthy){
98-
PX4_ERR("Checkpoint test failed with diff %.10f", abs_diff);
97+
PX4_ERR("Checkpoint test failed with diff %.10f", (double)abs_diff);
9998
return false;
10099
}
101100
else{
102-
PX4_INFO("Checkpoint test passed with diff %.10f", abs_diff);
101+
PX4_INFO("Checkpoint test passed with diff %.10f", (double)abs_diff);
103102
return true;
104103
}
105104
#else
@@ -182,11 +181,11 @@ bool Raptor::init()
182181
TI input_offset = 0;
183182
TI input_size = 0;
184183
rlt::persist::backends::tar::seek(device, reader_group.data, "example/input/data", input_offset, input_size);
185-
PX4_INFO("Input offset: %d", input_offset);
184+
PX4_INFO("Input offset: %d", (int)input_offset);
186185
TI output_offset = 0;
187186
TI output_size = 0;
188187
rlt::persist::backends::tar::seek(device, reader_group.data, "example/output/data", output_offset, output_size);
189-
PX4_INFO("Output offset: %d", output_offset);
188+
PX4_INFO("Output offset: %d", (int)output_offset);
190189
if(!test_policy(f, input_offset, output_offset)){
191190
PX4_ERR("Checkpoint test failed");
192191
return false;
@@ -225,14 +224,14 @@ bool Raptor::init()
225224

226225
int32_t imu_gyro_ratemax = _param_imu_gyro_ratemax.get();
227226
if(imu_gyro_ratemax % POLICY_CONTROL_FREQUENCY_TRAINING != 0){
228-
PX4_WARN("IMU_GYRO_RATEMAX=%d Hz is not a multiple of the training frequency (%d Hz)", imu_gyro_ratemax, POLICY_CONTROL_FREQUENCY_TRAINING);
227+
PX4_WARN("IMU_GYRO_RATEMAX=%d Hz is not a multiple of the training frequency (%d Hz)", (int)imu_gyro_ratemax, (int)POLICY_CONTROL_FREQUENCY_TRAINING);
229228
}
230229
int32_t force_sync_native = imu_gyro_ratemax / POLICY_CONTROL_FREQUENCY_TRAINING;
231230
executor.executor.force_sync_native = force_sync_native;
232231
executor.executor.force_sync_native_initialized = true;
233-
PX4_INFO("IMU_GYRO_RATEMAX=%d Hz", imu_gyro_ratemax);
234-
PX4_INFO("POLICY_CONTROL_FREQUENCY_TRAINING=%d Hz", POLICY_CONTROL_FREQUENCY_TRAINING);
235-
PX4_INFO("Setting force_sync_native = %d Hz / %d Hz = %d", imu_gyro_ratemax, POLICY_CONTROL_FREQUENCY_TRAINING, force_sync_native);
232+
PX4_INFO("IMU_GYRO_RATEMAX=%d Hz", (int)imu_gyro_ratemax);
233+
PX4_INFO("POLICY_CONTROL_FREQUENCY_TRAINING=%d Hz", (int)POLICY_CONTROL_FREQUENCY_TRAINING);
234+
PX4_INFO("Setting force_sync_native = %d Hz / %d Hz = %d", (int)imu_gyro_ratemax, (int)POLICY_CONTROL_FREQUENCY_TRAINING, (int)force_sync_native);
236235

237236
return true;
238237
}
@@ -300,10 +299,10 @@ void Raptor::observe(rl_tools::inference::applications::l2f::Observation<EXECUTO
300299
// diff = q2 - FRD2FLU * q * transpose(FRD2FLU)
301300
// @assert sum(abs.(diff)) < 1e-10
302301
T q_target[4];
303-
q_target[0] = cos(0.5 * _trajectory_setpoint.yaw); // minus because the setpoint yaw is in NED
302+
q_target[0] = cosf(0.5f * _trajectory_setpoint.yaw); // minus because the setpoint yaw is in NED
304303
q_target[1] = 0;
305304
q_target[2] = 0;
306-
q_target[3] = sin(0.5 * _trajectory_setpoint.yaw);
305+
q_target[3] = sinf(0.5f * _trajectory_setpoint.yaw);
307306

308307
T qt[4], qtc[4], qr[4];
309308
qt[0] = +q_target[0]; // conjugate to build the difference between setpoint and current
@@ -466,17 +465,22 @@ void Raptor::Run(){
466465
}
467466

468467
bool angular_velocity_update = false;
469-
if(status.subscription_update_angular_velocity = _vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity)){
468+
status.subscription_update_angular_velocity = _vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity);
469+
if(status.subscription_update_angular_velocity){
470470
timestamp_last_angular_velocity = current_time;
471471
timestamp_last_angular_velocity_set = true;
472472
angular_velocity_update = true;
473473
}
474474
status.timestamp_sample = _vehicle_angular_velocity.timestamp_sample;
475-
if(status.subscription_update_local_position = _vehicle_local_position_sub.update(&_vehicle_local_position)){
475+
476+
status.subscription_update_local_position = _vehicle_local_position_sub.update(&_vehicle_local_position);
477+
if(status.subscription_update_local_position){
476478
timestamp_last_local_position = current_time;
477479
timestamp_last_local_position_set = true;
478480
}
479-
if(status.subscription_update_visual_odometry = _vehicle_visual_odometry_sub.update(&_vehicle_visual_odometry)){
481+
482+
status.subscription_update_visual_odometry = _vehicle_visual_odometry_sub.update(&_vehicle_visual_odometry);
483+
if(status.subscription_update_visual_odometry){
480484
if(timestamp_last_visual_odometry_set){
481485
odometry_dts[odometry_dt_index] = current_time - timestamp_last_visual_odometry;
482486
odometry_dt_index = (odometry_dt_index + 1) % NUM_ODOMETRY_DTS;
@@ -506,7 +510,8 @@ void Raptor::Run(){
506510
status.visual_odometry_dt_mean /= NUM_ODOMETRY_DTS;
507511
status.visual_odometry_dt_std = sqrt(status.visual_odometry_dt_std / NUM_ODOMETRY_DTS - status.visual_odometry_dt_mean * status.visual_odometry_dt_mean);
508512

509-
if(status.subscription_update_attitude = _vehicle_attitude_sub.update(&_vehicle_attitude)){
513+
status.subscription_update_attitude = _vehicle_attitude_sub.update(&_vehicle_attitude);
514+
if(status.subscription_update_attitude){
510515
timestamp_last_attitude = current_time;
511516
timestamp_last_attitude_set = true;
512517
}
@@ -543,7 +548,7 @@ void Raptor::Run(){
543548
return;
544549
}
545550

546-
bool timestamp_last_odometry_set = Raptor::ODOMETRY_SOURCE == Raptor::OdometrySource::LOCAL_POSITION && timestamp_last_local_position_set || Raptor::ODOMETRY_SOURCE == Raptor::OdometrySource::VISUAL_ODOMETRY && timestamp_last_visual_odometry_set;
551+
bool timestamp_last_odometry_set = (Raptor::ODOMETRY_SOURCE == Raptor::OdometrySource::LOCAL_POSITION && timestamp_last_local_position_set) || (Raptor::ODOMETRY_SOURCE == Raptor::OdometrySource::VISUAL_ODOMETRY && timestamp_last_visual_odometry_set);
547552
if(!timestamp_last_angular_velocity_set || !timestamp_last_odometry_set || !timestamp_last_attitude_set){
548553
status.exit_reason = raptor_status_s::EXIT_REASON_NOT_ALL_OBSERVATIONS_SET;
549554
if constexpr(PUBLISH_NON_COMPLETE_STATUS){
@@ -600,7 +605,7 @@ void Raptor::Run(){
600605
tune_control.frequency = 1000;
601606
tune_control.duration = 10000;
602607
_tune_control_pub.publish(tune_control);
603-
PX4_WARN("VISUAL ODOMETRY STALE: Begin");
608+
PX4_WARN("Visual odometry stale: begin");
604609
}
605610
timestamp_last_visual_odometry_stale = timestamp_last_visual_odometry;
606611
timestamp_last_visual_odometry_stale_set = true;
@@ -629,7 +634,7 @@ void Raptor::Run(){
629634
tune_control.frequency = 2000;
630635
tune_control.duration = min(10000000, diff);
631636
_tune_control_pub.publish(tune_control);
632-
PX4_WARN("VISUAL ODOMETRY STALE: End %llu uS", diff);
637+
PX4_WARN("Visual odometry stale: end %llu uS", (unsigned long long)diff);
633638
}
634639
timestamp_last_visual_odometry_stale_set = false;
635640
if(Raptor::ODOMETRY_SOURCE == Raptor::OdometrySource::VISUAL_ODOMETRY){
@@ -691,7 +696,7 @@ void Raptor::Run(){
691696
if(!timestamp_last_trajectory_setpoint_set || (current_time - timestamp_last_trajectory_setpoint) > TRAJECTORY_SETPOINT_TIMEOUT){
692697
status.trajectory_setpoint_stale = true;
693698
if(!previous_trajectory_setpoint_stale){
694-
PX4_WARN("trajectory_setpoint turned stale at: %f %f %f", position[0], position[1], position[2]);
699+
PX4_WARN("trajectory_setpoint turned stale at: %f %f %f", (double)position[0], (double)position[1], (double)position[2]);
695700
_trajectory_setpoint.position[0] = position[0];
696701
_trajectory_setpoint.position[1] = position[1];
697702
_trajectory_setpoint.position[2] = position[2];
@@ -705,7 +710,7 @@ void Raptor::Run(){
705710
}
706711
else{
707712
if(previous_trajectory_setpoint_stale){
708-
PX4_WARN("trajectory_setpoint turned non-stale at: %f %f %f", position[0], position[1], position[2]);
713+
PX4_WARN("trajectory_setpoint turned non-stale at: %f %f %f", (double)position[0], (double)position[1], (double)position[2]);
709714
previous_trajectory_setpoint_stale = false;
710715
}
711716
status.trajectory_setpoint_stale = false;
@@ -715,7 +720,6 @@ void Raptor::Run(){
715720
rl_tools::inference::applications::l2f::Observation<EXECUTOR_SPEC> observation;
716721
rl_tools::inference::applications::l2f::Action<EXECUTOR_SPEC> action;
717722
observe(observation);
718-
// auto executor_status = rl_tools_inference_applications_l2f_control(current_time * 1000, &observation, &action);
719723
TI nanoseconds = current_time * 1000;
720724
auto executor_status = rl_tools::control(device, executor, nanoseconds, policy, observation, action, rng);
721725

@@ -727,8 +731,8 @@ void Raptor::Run(){
727731
return;
728732
}
729733

730-
731-
if (status.subscription_update_vehicle_status = _vehicle_status_sub.updated()) {
734+
status.subscription_update_vehicle_status = _vehicle_status_sub.updated();
735+
if(status.subscription_update_vehicle_status) {
732736
_vehicle_status_sub.copy(&_vehicle_status);
733737
}
734738
bool next_active = _vehicle_status.nav_state == ext_component_mode_id;
@@ -764,7 +768,7 @@ void Raptor::Run(){
764768
_raptor_input_pub.publish(input_msg);
765769
_raptor_status_pub.publish(status);
766770

767-
actuator_motors_s actuator_motors = {}; // zero initialize to set e.g. the reversible_flags to all 0
771+
actuator_motors_s actuator_motors{};
768772
actuator_motors.timestamp = hrt_absolute_time();
769773
actuator_motors.timestamp_sample = _vehicle_angular_velocity.timestamp_sample;
770774
for(TI action_i=0; action_i < actuator_motors_s::NUM_CONTROLS; action_i++){
@@ -813,7 +817,7 @@ void Raptor::Run(){
813817
if(this->timestamp_last_policy_frequency_check_set){
814818
if(last_intermediate_status_set){
815819
if(!this->last_intermediate_status.timing_bias.OK || !this->last_intermediate_status.timing_jitter.OK){
816-
PX4_WARN("Raptor: INTERMEDIATE: BIAS %fx JITTER %fx", this->last_intermediate_status.timing_bias.MAGNITUDE, this->last_intermediate_status.timing_jitter.MAGNITUDE);
820+
PX4_WARN("Raptor: INTERMEDIATE: BIAS %fx JITTER %fx", (double)this->last_intermediate_status.timing_bias.MAGNITUDE, (double)this->last_intermediate_status.timing_jitter.MAGNITUDE);
817821
}
818822
else{
819823
if(this->policy_frequency_check_counter % POLICY_FREQUENCY_INFO_INTERVAL == 0){
@@ -823,7 +827,7 @@ void Raptor::Run(){
823827
}
824828
if(last_native_status_set){
825829
if(!this->last_native_status.timing_bias.OK || !this->last_native_status.timing_jitter.OK){
826-
PX4_WARN("Raptor: NATIVE: BIAS %fx JITTER %fx", this->last_native_status.timing_bias.MAGNITUDE, this->last_native_status.timing_jitter.MAGNITUDE);
830+
PX4_WARN("Raptor: NATIVE: BIAS %fx JITTER %fx", (double)this->last_native_status.timing_bias.MAGNITUDE, (double)this->last_native_status.timing_jitter.MAGNITUDE);
827831
}
828832
else{
829833
if(this->policy_frequency_check_counter % POLICY_FREQUENCY_INFO_INTERVAL == 0){

‎src/modules/mc_raptor/module.yaml‎

Lines changed: 10 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,15 @@ parameters:
99
When enabled, the Raptor flight mode will be available. Please set MC_RAPTOR_OFFB according to your use case.
1010
type: boolean
1111
default: false
12-
category: System
12+
category: Raptor
13+
MC_RAPTOR_VERBOS:
14+
description:
15+
short: Enable verbose output
16+
long: |
17+
When enabled, the Raptor will print verbose output to the console.
18+
type: boolean
19+
default: false
20+
category: Raptor
1321

1422
MC_RAPTOR_OFFB:
1523
description:
@@ -19,5 +27,5 @@ parameters:
1927
If disabled, the Raptor mode will be available as a separate external mode. In the latter case, Raptor will just hold the position, without requiring external setpoints. When Raptor replaces the Offboard mode, it requires external setpoints to be activated.
2028
type: boolean
2129
default: false
22-
category: System
30+
category: Raptor
2331

0 commit comments

Comments
 (0)