diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index e797a52d58..1a575358f3 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -1,5 +1,22 @@ #include "Copter.h" +/************************************************************* + * Attitude Rate controllers and timing + ****************************************************************/ + +// update rate controllers and output to roll, pitch and yaw actuators +// called at 400hz by default +void Copter::run_rate_controller() +{ + // set attitude and position controller loop time + const float last_loop_time_s = AP::scheduler().get_last_loop_time_s(); + attitude_control->set_dt(last_loop_time_s); + pos_control->set_dt(last_loop_time_s); + + // run low level rate controllers that only require IMU data + attitude_control->rate_controller_run(); +} + /************************************************************* * throttle control ****************************************************************/ diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 47e04ac454..039f28c674 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -696,7 +696,7 @@ private: void set_accel_throttle_I_from_pilot_throttle(); void rotate_body_frame_to_NE(float &x, float &y); uint16_t get_pilot_speed_dn() const; - void run_rate_controller() { attitude_control->rate_controller_run(); } + void run_rate_controller(); #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED void run_custom_controller() { custom_control.update(); } diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 20ee805e9a..27ff27a212 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -448,15 +448,15 @@ void Copter::allocate_motors(void) #if FRAME_CONFIG != HELI_FRAME if ((AP_Motors::motor_frame_class)g2.frame_class.get() == AP_Motors::MOTOR_FRAME_6DOF_SCRIPTING) { #if AP_SCRIPTING_ENABLED - attitude_control = new AC_AttitudeControl_Multi_6DoF(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s()); + attitude_control = new AC_AttitudeControl_Multi_6DoF(*ahrs_view, aparm, *motors); ac_var_info = AC_AttitudeControl_Multi_6DoF::var_info; #endif // AP_SCRIPTING_ENABLED } else { - attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s()); + attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors); ac_var_info = AC_AttitudeControl_Multi::var_info; } #else - attitude_control = new AC_AttitudeControl_Heli(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s()); + attitude_control = new AC_AttitudeControl_Heli(*ahrs_view, aparm, *motors); ac_var_info = AC_AttitudeControl_Heli::var_info; #endif if (attitude_control == nullptr) { @@ -464,7 +464,7 @@ void Copter::allocate_motors(void) } AP_Param::load_object_from_eeprom(attitude_control, ac_var_info); - pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control, scheduler.get_loop_period_s()); + pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control); if (pos_control == nullptr) { AP_BoardConfig::allocation_error("PosControl"); }