From a0f8d26c8a1b0682731d053d629a6dd901a49506 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Fri, 3 May 2024 23:04:01 -0400 Subject: [PATCH] Copter: fix compile errors --- ArduCopter/Attitude.cpp | 2 +- ArduCopter/RC_Channel.cpp | 4 ++-- ArduCopter/mode.h | 8 ++++---- ArduCopter/mode_quadsquad.cpp | 14 +++++++------- ArduCopter/mode_systemid.cpp | 8 +++----- 5 files changed, 17 insertions(+), 19 deletions(-) diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 667e2391cbf855..90f74be9b900fa 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -15,7 +15,7 @@ void Copter::run_rate_controller() pos_control->set_dt(last_loop_time_s); // Bypass attitude controller if quadsquad is current flight mode. quadsquad is self contained - if (control_mode != Mode::Number::QUADSQUAD && control_mode != Mode::Number::SYSTEMID) { + if (flightmode->mode_number() != Mode::Number::QUADSQUAD && flightmode->mode_number() != Mode::Number::SYSTEMID) { // run low level rate controllers that only require IMU data attitude_control->rate_controller_run(); } diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index 5692677dae9535..3c2fb30402b865 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -461,8 +461,8 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi case AUX_FUNC::USER_FUNC1: copter.userhook_auxSwitch1(ch_flag); switch (ch_flag) { - case HIGH: - case MIDDLE: + case AuxSwitchPos::HIGH: + case AuxSwitchPos::MIDDLE: copter.mode_quadsquad.set_traj_sw(2); gcs().send_text(MAV_SEVERITY_INFO, "quadsquad trajectory started"); break; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index aa2bcef3422434..f16ec1e4f9d204 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1599,7 +1599,7 @@ class ModeSystemId : public Mode { bool has_manual_throttle() const override { return true; } bool allows_arming(AP_Arming::Method method) const override { return false; }; bool is_autopilot() const override { return false; } - bool logs_attitude() const override { return false } + bool logs_attitude() const override { return false; } void set_magnitude(float input) { waveform_magnitude.set(input); } void set_traj_sw(uint8_t sw_status) { traj_sw = sw_status; } @@ -2005,16 +2005,16 @@ class ModeAutorotate : public Mode { class ModeQuadsquad : public Mode { public: - // inherit constructor -// using Mode::Mode; + ModeQuadsquad(void); + Number mode_number() const override { return Number::QUADSQUAD; } virtual void run() override; bool init(bool ignore_checks) override; bool requires_GPS() const override { return false; } bool has_manual_throttle() const override { return true; } - bool allows_arming(bool from_gcs) const override { return true; }; + bool allows_arming(AP_Arming::Method method) const override { return true; }; bool is_autopilot() const override { return false; } void set_traj_sw(uint8_t sw_status) { traj_sw = sw_status; } diff --git a/ArduCopter/mode_quadsquad.cpp b/ArduCopter/mode_quadsquad.cpp index ac17d7355a57f9..9fcd9c471c2798 100644 --- a/ArduCopter/mode_quadsquad.cpp +++ b/ArduCopter/mode_quadsquad.cpp @@ -25,7 +25,7 @@ ModeQuadsquad::ModeQuadsquad(void) : Mode() bool ModeQuadsquad::init(bool ignore_checks) { // set target altitude to zero for reporting - pos_control->set_alt_target(0); + pos_control->set_alt_target_with_slew(0.0f); QS_InnerRateLoop_Obj.initialize(); engage=0; VeSqrSum=0; @@ -59,8 +59,8 @@ void ModeQuadsquad::run() const Vector3f &accel = copter.ins.get_accel(); const Vector3f &gyro = copter.ins.get_gyro(); - const Vector3f &curr_pos = inertial_nav.get_position(); - const Vector3f &curr_vel = inertial_nav.get_velocity(); + const Vector3f &curr_pos = inertial_nav.get_position_neu_cm(); + const Vector3f &curr_vel = inertial_nav.get_velocity_neu_cms(); // apply simple mode transform to pilot inputs update_simple_mode(); @@ -78,13 +78,13 @@ void ModeQuadsquad::run() switch (motors->get_spool_state()) { case AP_Motors::SpoolState::SHUT_DOWN: // Motors Stopped - attitude_control->set_yaw_target_to_current_heading(); + attitude_control->reset_yaw_target_and_rate(); attitude_control->reset_rate_controller_I_terms(); break; case AP_Motors::SpoolState::GROUND_IDLE: // Landed - attitude_control->set_yaw_target_to_current_heading(); + attitude_control->reset_yaw_target_and_rate(); attitude_control->reset_rate_controller_I_terms_smoothly(); break; @@ -150,8 +150,8 @@ void ModeQuadsquad::run() // Trajectory on or off QS_InnerRateLoop_Obj.QS_InnerRateLoop_U.TrajectorySwitch = traj_sw; - - alpha = 1.0; // hard coding alpha + alpha.set(1.0f); +// alpha = 1.0f; // hard coding alpha //Scale RV if(alpha <= 0 || alpha > 3 ){ QS_InnerRateLoop_Obj.QS_InnerRateLoop_U.Rv = 2; diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index feeceb2e13b7ff..8b9af4014f1718 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -101,9 +101,7 @@ bool ModeSystemId::init(bool ignore_checks) #if HAL_LOGGING_ENABLED copter.Log_Write_SysID_Setup(axis, waveform_magnitude, frequency_start, frequency_stop, time_fade_in, time_const_freq, time_record, time_fade_out); #endif - - // set target altitude to zero for reporting - pos_control->set_alt_target(0); + pos_control->set_alt_target_with_slew(0.0f); QS_InnerRateLoop_Obj.initialize(); engage=0; VeSqrSum=0; @@ -136,8 +134,8 @@ void ModeSystemId::run() const Vector3f &accel = copter.ins.get_accel(); const Vector3f &gyro = copter.ins.get_gyro(); - const Vector3f &curr_pos = inertial_nav.get_position(); - const Vector3f &curr_vel = inertial_nav.get_velocity(); + const Vector3f &curr_pos = inertial_nav.get_position_neu_cm(); + const Vector3f &curr_vel = inertial_nav.get_velocity_neu_cms(); // system ID inputs float input_pitch = 0.0f;