From 5df8dd8c5058a4d94c7cde11e5138064911c5d4c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 30 Mar 2021 14:36:25 +1100 Subject: [PATCH] Copter: move guided submode enumeration into ModeGuided --- ArduCopter/Copter.h | 2 +- ArduCopter/GCS_Mavlink.cpp | 8 ++--- ArduCopter/Log.cpp | 6 ++-- ArduCopter/defines.h | 9 ------ ArduCopter/mode.h | 12 +++++-- ArduCopter/mode_guided.cpp | 64 ++++++++++++++++++++++---------------- 6 files changed, 56 insertions(+), 45 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 8874771f6c..3b9ace2b50 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -791,7 +791,7 @@ private: #if FRAME_CONFIG == HELI_FRAME void Log_Write_Heli(void); #endif - void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); + void Log_Write_GuidedTarget(ModeGuided::SubMode submode, const Vector3f& pos_target, const Vector3f& vel_target); void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out); void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z); void Log_Write_Vehicle_Startup_Messages(); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 4cf0898390..a26dfa3fff 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -126,16 +126,16 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned() if (!copter.flightmode->in_guided_mode()) { return; } - - const GuidedMode guided_mode = copter.mode_guided.mode(); + + const ModeGuided::SubMode guided_mode = copter.mode_guided.submode(); Vector3f target_pos; Vector3f target_vel; uint16_t type_mask; - if (guided_mode == Guided_WP) { + if (guided_mode == ModeGuided::SubMode::WP) { type_mask = 0x0FF8; // ignore everything except position target_pos = copter.wp_nav->get_wp_destination() * 0.01f; // convert to metres - } else if (guided_mode == Guided_Velocity) { + } else if (guided_mode == ModeGuided::SubMode::Velocity) { type_mask = 0x0FC7; // ignore everything except velocity target_vel = copter.flightmode->get_desired_velocity() * 0.01f; // convert to m/s } else { diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index b1be664e4c..6192f3e615 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -382,12 +382,12 @@ struct PACKED log_GuidedTarget { // Write a Guided mode target // pos_target is lat, lon, alt OR offset from ekf origin in cm OR roll, pitch, yaw target in centi-degrees // vel_target is cm/s -void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) +void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vector3f& pos_target, const Vector3f& vel_target) { struct log_GuidedTarget pkt = { LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG), time_us : AP_HAL::micros64(), - type : target_type, + type : (uint8_t)target_type, pos_target_x : pos_target.x, pos_target_y : pos_target.y, pos_target_z : pos_target.z, @@ -571,7 +571,7 @@ void Copter::Log_Write_Data(LogDataID id, uint16_t value) {} void Copter::Log_Write_Data(LogDataID id, float value) {} void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {} void Copter::Log_Sensor_Health() {} -void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {} +void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode&, const Vector3f& pos_target, const Vector3f& vel_target) {} void Copter::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out) {} void Copter::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) {} void Copter::Log_Write_Vehicle_Startup_Messages() {} diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 5ebb5bd25d..cd8763b15b 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -97,15 +97,6 @@ enum AutoMode { Auto_NavPayloadPlace, }; -// Guided modes -enum GuidedMode { - Guided_TakeOff, - Guided_WP, - Guided_Velocity, - Guided_PosVel, - Guided_Angle, -}; - // Airmode enum class AirMode { AIRMODE_NONE, diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 0f4a566337..6c84033401 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -846,7 +846,15 @@ public: bool do_user_takeoff_start(float takeoff_alt_cm) override; - GuidedMode mode() const { return guided_mode; } + enum class SubMode { + TakeOff, + WP, + Velocity, + PosVel, + Angle, + }; + + SubMode submode() const { return guided_mode; } void angle_control_start(); void angle_control_run(); @@ -881,7 +889,7 @@ private: bool use_pilot_yaw(void) const; // controls which controller is run (pos or vel): - GuidedMode guided_mode = Guided_TakeOff; + SubMode guided_mode = SubMode::TakeOff; bool send_notification; // used to send one time notification to ground station }; diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index ecd677c2fb..b66d49d8a2 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -55,12 +55,12 @@ void ModeGuided::run() // call the correct auto controller switch (guided_mode) { - case Guided_TakeOff: + case SubMode::TakeOff: // run takeoff controller takeoff_run(); break; - case Guided_WP: + case SubMode::WP: // run position controller pos_control_run(); if (send_notification && wp_nav->reached_wp_destination()) { @@ -69,17 +69,17 @@ void ModeGuided::run() } break; - case Guided_Velocity: + case SubMode::Velocity: // run velocity controller vel_control_run(); break; - case Guided_PosVel: + case SubMode::PosVel: // run position-velocity controller posvel_control_run(); break; - case Guided_Angle: + case SubMode::Angle: // run angle controller angle_control_run(); break; @@ -100,7 +100,7 @@ bool ModeGuided::allows_arming(AP_Arming::Method method) const // do_user_takeoff_start - initialises waypoint controller to implement take-off bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm) { - guided_mode = Guided_TakeOff; + guided_mode = SubMode::TakeOff; // initialise wpnav destination Location target_loc = copter.current_loc; @@ -139,7 +139,7 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm) void ModeGuided::pos_control_start() { // set to position control mode - guided_mode = Guided_WP; + guided_mode = SubMode::WP; // initialise waypoint and spline controller wp_nav->wp_and_spline_init(); @@ -159,7 +159,7 @@ void ModeGuided::pos_control_start() void ModeGuided::vel_control_start() { // set guided_mode to velocity controller - guided_mode = Guided_Velocity; + guided_mode = SubMode::Velocity; // initialise horizontal speed, acceleration pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy()); @@ -177,7 +177,7 @@ void ModeGuided::vel_control_start() void ModeGuided::posvel_control_start() { // set guided_mode to velocity controller - guided_mode = Guided_PosVel; + guided_mode = SubMode::PosVel; pos_control->init_xy_controller(); @@ -202,14 +202,14 @@ void ModeGuided::posvel_control_start() bool ModeGuided::is_taking_off() const { - return guided_mode == Guided_TakeOff; + return guided_mode == SubMode::TakeOff; } // initialise guided mode's angle controller void ModeGuided::angle_control_start() { // set guided_mode to velocity controller - guided_mode = Guided_Angle; + guided_mode = SubMode::Angle; // set vertical speed and acceleration pos_control->set_max_speed_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up()); @@ -250,7 +250,7 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa #endif // ensure we are in position control mode - if (guided_mode != Guided_WP) { + if (guided_mode != SubMode::WP) { pos_control_start(); } @@ -270,7 +270,7 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa bool ModeGuided::get_wp(Location& destination) { - if (guided_mode != Guided_WP) { + if (guided_mode != SubMode::WP) { return false; } return wp_nav->get_oa_wp_destination(destination); @@ -292,7 +292,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y #endif // ensure we are in position control mode - if (guided_mode != Guided_WP) { + if (guided_mode != SubMode::WP) { pos_control_start(); } @@ -318,7 +318,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y void ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request) { // check we are in velocity control mode - if (guided_mode != Guided_Velocity) { + if (guided_mode != SubMode::Velocity) { vel_control_start(); } @@ -349,7 +349,7 @@ bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vecto #endif // check we are in velocity control mode - if (guided_mode != Guided_PosVel) { + if (guided_mode != SubMode::PosVel) { posvel_control_start(); } @@ -371,7 +371,7 @@ bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vecto void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, bool use_yaw_rate, float yaw_rate_rads, bool use_thrust) { // check we are in velocity control mode - if (guided_mode != Guided_Angle) { + if (guided_mode != SubMode::Angle) { angle_control_start(); } @@ -784,11 +784,11 @@ bool ModeGuided::limit_check() uint32_t ModeGuided::wp_distance() const { - switch(mode()) { - case Guided_WP: + switch(guided_mode) { + case SubMode::WP: return wp_nav->get_wp_distance_to_destination(); break; - case Guided_PosVel: + case SubMode::PosVel: return pos_control->get_pos_error_xy(); break; default: @@ -798,25 +798,37 @@ uint32_t ModeGuided::wp_distance() const int32_t ModeGuided::wp_bearing() const { - switch(mode()) { - case Guided_WP: + switch(guided_mode) { + case SubMode::WP: return wp_nav->get_wp_bearing_to_destination(); break; - case Guided_PosVel: + case SubMode::PosVel: return pos_control->get_bearing_to_target(); break; - default: + case SubMode::TakeOff: + case SubMode::Velocity: + case SubMode::Angle: + // these do not have bearings return 0; } + // compiler guarantees we don't get here + return 0.0; } float ModeGuided::crosstrack_error() const { - if (mode() == Guided_WP) { + switch (guided_mode) { + case SubMode::WP: return wp_nav->crosstrack_error(); - } else { + case SubMode::TakeOff: + case SubMode::Velocity: + case SubMode::PosVel: + case SubMode::Angle: + // no track to have a crosstrack to return 0; } + // compiler guarantees we don't get here + return 0; } #endif