Changing the Frequency of the Position Controller to 100Hz

mitchell · October 4, 2023, 2:11am

Hello,

I’m interested in adjusting the ‘mc_pos_control’ module frequency from 50 Hz to 100 Hz. I’ve noticed that the ‘mc_pos_control’ module is added as the ‘nav_and_controllers’ work queue, but I’m unsure about where or how to modify its actual run frequency.

Could anyone guide me on where and how I can make this change to set the position controller’s frequency to 100Hz? Any insights or pointers would be greatly appreciated.

Thank you in advance for your assistance!

JulianOes · October 5, 2023, 4:51am

The MC position controller registers on the local position topic:

github.com

PX4/PX4-Autopilot/blob/cad595cb5c599522a242e3469f4e5ecb7c74ff50/src/modules/mc_pos_control/MulticopterPositionControl.cpp#L65


      
          	_takeoff_status_pub.advertise();
          }
          
          MulticopterPositionControl::~MulticopterPositionControl()
          {
          	perf_free(_cycle_perf);
          }
          
          bool MulticopterPositionControl::init()
          {
          	if (!_local_pos_sub.registerCallback()) {
          		PX4_ERR("callback registration failed");
          		return false;
          	}
          
          	_time_stamp_last_loop = hrt_absolute_time();
          	ScheduleNow();
          
          	return true;
          }

The local position seems to be published in the EKF2:

github.com

PX4/PX4-Autopilot/blob/cad595cb5c599522a242e3469f4e5ecb7c74ff50/src/modules/ekf2/EKF2.cpp#L1642


      
          	if (!PX4_ISFINITE(lpos.hagl_min)) {
          		lpos.hagl_min = INFINITY;
          	}
          
          	if (!PX4_ISFINITE(lpos.hagl_max)) {
          		lpos.hagl_max = INFINITY;
          	}
          
          	// publish vehicle local position data
          	lpos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
          	_local_position_pub.publish(lpos);
          }
          
          void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sample)
          {
          	// generate vehicle odometry data
          	vehicle_odometry_s odom;
          	odom.timestamp_sample = imu_sample.time_us;
          
          	// position
          	odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;

Which is triggered after each EKF update:

github.com

PX4/PX4-Autopilot/blob/cad595cb5c599522a242e3469f4e5ecb7c74ff50/src/modules/ekf2/EKF2.cpp#L732C8-L732C8


      
          			PublishLocalPosition(now);

If I check uorb top on my bench setup, it tells me that vehicle_local_position is running at 100 Hz, so I would expect the position controller to work at that same rate . Can you verify that?

mitchell · October 8, 2023, 1:26am

Thank you very much for your detailed guide, Julian.
Yes, I was able to check that Pos, att, and rate modules run at 100Hz, 200Hz, and 800Hz, respectively, using CUAV Nora with the latest px4 firmware.
px4

So I believe the controller diagram in the PX4 User Guide is inaccurate?