Skip to content

Commit

Permalink
Copter: use base class AC_AttitudeControl object
Browse files Browse the repository at this point in the history
  • Loading branch information
IamPete1 committed Dec 3, 2023
1 parent a0a113c commit b099c30
Show file tree
Hide file tree
Showing 4 changed files with 8 additions and 19 deletions.
9 changes: 2 additions & 7 deletions ArduCopter/Copter.h
Original file line number Diff line number Diff line change
Expand Up @@ -77,12 +77,6 @@
#include "defines.h"
#include "config.h"

#if FRAME_CONFIG == HELI_FRAME
#define AC_AttitudeControl_t AC_AttitudeControl_Heli
#else
#define AC_AttitudeControl_t AC_AttitudeControl_Multi
#endif

#if FRAME_CONFIG == HELI_FRAME
#define MOTOR_CLASS AP_MotorsHeli
#else
Expand Down Expand Up @@ -485,7 +479,8 @@ class Copter : public AP_Vehicle {

// Attitude, Position and Waypoint navigation objects
// To-Do: move inertial nav up or other navigation variables down here
AC_AttitudeControl_t *attitude_control;
AC_AttitudeControl *attitude_control;
const struct AP_Param::GroupInfo *attitude_control_var_info;
AC_PosControl *pos_control;
AC_WPNav *wp_nav;
AC_Loiter *loiter_nav;
Expand Down
6 changes: 1 addition & 5 deletions ArduCopter/Parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -498,11 +498,7 @@ const AP_Param::Info Copter::var_info[] = {

// @Group: ATC_
// @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
#if FRAME_CONFIG == HELI_FRAME
GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Heli),
#else
GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Multi),
#endif
GOBJECTVARPTR(attitude_control, "ATC_", &copter.attitude_control_var_info),

// @Group: PSC
// @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp
Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/mode.h
Original file line number Diff line number Diff line change
Expand Up @@ -241,7 +241,7 @@ class Mode {
AC_PosControl *&pos_control;
AP_InertialNav &inertial_nav;
AP_AHRS &ahrs;
AC_AttitudeControl_t *&attitude_control;
AC_AttitudeControl *&attitude_control;
MOTOR_CLASS *&motors;
RC_Channel *&channel_roll;
RC_Channel *&channel_pitch;
Expand Down
10 changes: 4 additions & 6 deletions ArduCopter/system.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -444,26 +444,24 @@ void Copter::allocate_motors(void)
AP_BoardConfig::allocation_error("AP_AHRS_View");
}

const struct AP_Param::GroupInfo *ac_var_info;

#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);
ac_var_info = AC_AttitudeControl_Multi_6DoF::var_info;
attitude_control_var_info = AC_AttitudeControl_Multi_6DoF::var_info;
#endif // AP_SCRIPTING_ENABLED
} else {
attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors);
ac_var_info = AC_AttitudeControl_Multi::var_info;
attitude_control_var_info = AC_AttitudeControl_Multi::var_info;
}
#else
attitude_control = new AC_AttitudeControl_Heli(*ahrs_view, aparm, *motors);
ac_var_info = AC_AttitudeControl_Heli::var_info;
attitude_control_var_info = AC_AttitudeControl_Heli::var_info;
#endif
if (attitude_control == nullptr) {
AP_BoardConfig::allocation_error("AttitudeControl");
}
AP_Param::load_object_from_eeprom(attitude_control, ac_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);
if (pos_control == nullptr) {
Expand Down

0 comments on commit b099c30

Please sign in to comment.