Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: Support changing update period

This commit is contained in:
Leonard Hall 2022-11-30 15:57:50 +10:30 committed by Andrew Tridgell
parent eb67c92852
commit f0a9b96eca
2 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/Attitude.cpp
View File

@ -760,8 +760,7 @@ void Plane::calc_nav_roll()
float bank_limit = degrees(atanf(guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f; float bank_limit = degrees(atanf(guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f;
g2.guidedHeading.update_error(error); // push error into AC_PID , possible improvement is to use update_all instead.? g2.guidedHeading.update_error(error, delta); // push error into AC_PID , possible improvement is to use update_all instead.?
g2.guidedHeading.set_dt(delta);
float i = g2.guidedHeading.get_i(); // get integrator TODO float i = g2.guidedHeading.get_i(); // get integrator TODO
if (((is_negative(error) && !guided_state.target_heading_limit_low) || (is_positive(error) && !guided_state.target_heading_limit_high))) { if (((is_negative(error) && !guided_state.target_heading_limit_low) || (is_positive(error) && !guided_state.target_heading_limit_high))) {

8
ArduPlane/quadplane.cpp
View File

@ -607,7 +607,6 @@ bool QuadPlane::setup(void)
if (!enable || hal.util->get_soft_armed()) { if (!enable || hal.util->get_soft_armed()) {
return false; return false;
} }
float loop_delta_t = 1.0 / plane.scheduler.get_loop_rate_hz();
/* /*
cope with upgrade from old AP_Motors values for frame_class cope with upgrade from old AP_Motors values for frame_class
@ -716,13 +715,13 @@ bool QuadPlane::setup(void)
AP_BoardConfig::allocation_error("ahrs_view"); AP_BoardConfig::allocation_error("ahrs_view");
} }
attitude_control = new AC_AttitudeControl_TS(*ahrs_view, aparm, *motors, loop_delta_t); attitude_control = new AC_AttitudeControl_TS(*ahrs_view, aparm, *motors);
if (!attitude_control) { if (!attitude_control) {
AP_BoardConfig::allocation_error("attitude_control"); AP_BoardConfig::allocation_error("attitude_control");
} }
AP_Param::load_object_from_eeprom(attitude_control, attitude_control->var_info); AP_Param::load_object_from_eeprom(attitude_control, attitude_control->var_info);
pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control, loop_delta_t); pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control);
if (!pos_control) { if (!pos_control) {
AP_BoardConfig::allocation_error("pos_control"); AP_BoardConfig::allocation_error("pos_control");
} }
@ -1986,6 +1985,9 @@ void QuadPlane::motors_output(bool run_rate_controller)
// relax if have been inactive // relax if have been inactive
relax_attitude_control(); relax_attitude_control();
} }
// run low level rate controllers that only require IMU data and set loop time
attitude_control->set_dt(AP::scheduler().get_last_loop_time_s());
pos_control->set_dt(AP::scheduler().get_last_loop_time_s());
attitude_control->rate_controller_run(); attitude_control->rate_controller_run();
last_att_control_ms = now; last_att_control_ms = now;
} }