diff --git a/ArduSub/AP_Arming_Sub.cpp b/ArduSub/AP_Arming_Sub.cpp index 7eb1589a81..84789a10f6 100644 --- a/ArduSub/AP_Arming_Sub.cpp +++ b/ArduSub/AP_Arming_Sub.cpp @@ -134,7 +134,7 @@ bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks) sub.motors.armed(true); // log flight mode in case it was changed while vehicle was disarmed - AP::logger().Write_Mode(sub.control_mode, sub.control_mode_reason); + AP::logger().Write_Mode((uint8_t)sub.control_mode, sub.control_mode_reason); // reenable failsafe sub.mainloop_failsafe_enable(); diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 082b44afa9..9869ed29b8 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -140,7 +140,7 @@ void Sub::run_rate_controller() pos_control.set_dt(last_loop_time_s); //don't run rate controller in manual or motordetection modes - if (control_mode != MANUAL && control_mode != MOTOR_DETECT) { + if (control_mode != Mode::Number::MANUAL && control_mode != Mode::Number::MOTOR_DETECT) { // run low level rate controllers that only require IMU data and set loop time attitude_control.rate_controller_run(); } @@ -201,7 +201,7 @@ void Sub::ten_hz_logging_loop() if (should_log(MASK_LOG_RCOUT)) { logger.Write_RCOUT(); } - if (should_log(MASK_LOG_NTUN) && (mode_requires_GPS(control_mode) || !mode_has_manual_throttle(control_mode))) { + if (should_log(MASK_LOG_NTUN) && (sub.flightmode->requires_GPS() || !sub.flightmode->has_manual_throttle())) { pos_control.write_log(); } if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) { diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index f078629041..f4ed519b5b 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -21,10 +21,10 @@ MAV_MODE GCS_MAVLINK_Sub::base_mode() const // the APM flight mode and has a well defined meaning in the // ArduPlane documentation switch (sub.control_mode) { - case AUTO: - case GUIDED: - case CIRCLE: - case POSHOLD: + case Mode::Number::AUTO: + case Mode::Number::GUIDED: + case Mode::Number::CIRCLE: + case Mode::Number::POSHOLD: _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what // APM does in any mode, as that is defined as "system finds its own goal @@ -50,7 +50,7 @@ MAV_MODE GCS_MAVLINK_Sub::base_mode() const uint32_t GCS_Sub::custom_mode() const { - return sub.control_mode; + return (uint32_t)sub.control_mode; } MAV_STATE GCS_MAVLINK_Sub::vehicle_system_status() const @@ -448,7 +448,7 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_do_set_roi(const Location &roi_loc) if (!roi_loc.check_latlng()) { return MAV_RESULT_FAILED; } - sub.set_auto_yaw_roi(roi_loc); + sub.mode_auto.set_auto_yaw_roi(roi_loc); return MAV_RESULT_ACCEPTED; } @@ -464,13 +464,13 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon { switch (packet.command) { case MAV_CMD_NAV_LOITER_UNLIM: - if (!sub.set_mode(POSHOLD, ModeReason::GCS_COMMAND)) { + if (!sub.set_mode(Mode::Number::POSHOLD, ModeReason::GCS_COMMAND)) { return MAV_RESULT_FAILED; } return MAV_RESULT_ACCEPTED; case MAV_CMD_NAV_LAND: - if (!sub.set_mode(SURFACE, ModeReason::GCS_COMMAND)) { + if (!sub.set_mode(Mode::Number::SURFACE, ModeReason::GCS_COMMAND)) { return MAV_RESULT_FAILED; } return MAV_RESULT_ACCEPTED; @@ -483,7 +483,7 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon if ((packet.param1 >= 0.0f) && (packet.param1 <= 360.0f) && (is_zero(packet.param4) || is_equal(packet.param4,1.0f))) { - sub.set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, (uint8_t)packet.param4); + sub.mode_auto.set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, (uint8_t)packet.param4); return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; @@ -500,7 +500,7 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon return MAV_RESULT_FAILED; case MAV_CMD_MISSION_START: - if (sub.motors.armed() && sub.set_mode(AUTO, ModeReason::GCS_COMMAND)) { + if (sub.motors.armed() && sub.set_mode(Mode::Number::AUTO, ModeReason::GCS_COMMAND)) { return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; @@ -588,7 +588,7 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) // descend at up to WPNAV_SPEED_DN climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * sub.wp_nav.get_default_speed_down(); } - sub.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms); + sub.mode_guided.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms); break; } @@ -598,7 +598,7 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) mavlink_msg_set_position_target_local_ned_decode(&msg, &packet); // exit if vehicle is not in Guided mode or Auto-Guided mode - if ((sub.control_mode != GUIDED) && !(sub.control_mode == AUTO && sub.auto_mode == Auto_NavGuided)) { + if ((sub.control_mode != Mode::Number::GUIDED) && !(sub.control_mode == Mode::Number::AUTO && sub.auto_mode == Auto_NavGuided)) { break; } @@ -652,11 +652,11 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) // send request if (!pos_ignore && !vel_ignore && acc_ignore) { - sub.guided_set_destination_posvel(pos_vector, vel_vector); + sub.mode_guided.guided_set_destination_posvel(pos_vector, vel_vector); } else if (pos_ignore && !vel_ignore && acc_ignore) { - sub.guided_set_velocity(vel_vector); + sub.mode_guided.guided_set_velocity(vel_vector); } else if (!pos_ignore && vel_ignore && acc_ignore) { - sub.guided_set_destination(pos_vector); + sub.mode_guided.guided_set_destination(pos_vector); } break; @@ -668,9 +668,9 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) mavlink_msg_set_position_target_global_int_decode(&msg, &packet); // exit if vehicle is not in Guided, Auto-Guided, or Depth Hold modes - if ((sub.control_mode != GUIDED) - && !(sub.control_mode == AUTO && sub.auto_mode == Auto_NavGuided) - && !(sub.control_mode == ALT_HOLD)) { + if ((sub.control_mode != Mode::Number::GUIDED) + && !(sub.control_mode == Mode::Number::AUTO && sub.auto_mode == Auto_NavGuided) + && !(sub.control_mode == Mode::Number::ALT_HOLD)) { break; } @@ -686,7 +686,7 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) * bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; */ - if (!z_ignore && sub.control_mode == ALT_HOLD) { // Control only target depth when in ALT_HOLD + if (!z_ignore && sub.control_mode == Mode::Number::ALT_HOLD) { // Control only target depth when in ALT_HOLD sub.pos_control.set_pos_target_z_cm(packet.alt*100); break; } @@ -715,11 +715,11 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) } if (!pos_ignore && !vel_ignore && acc_ignore) { - sub.guided_set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); + sub.mode_guided.guided_set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); } else if (pos_ignore && !vel_ignore && acc_ignore) { - sub.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); + sub.mode_guided.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f)); } else if (!pos_ignore && vel_ignore && acc_ignore) { - sub.guided_set_destination(pos_neu_cm); + sub.mode_guided.guided_set_destination(pos_neu_cm); } break; @@ -794,7 +794,7 @@ int16_t GCS_MAVLINK_Sub::high_latency_target_altitude() const UNUSED_RESULT(ahrs.get_location(global_position_current)); //return units are m - if (sub.control_mode == AUTO || sub.control_mode == GUIDED) { + if (sub.control_mode == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) { return 0.01 * (global_position_current.alt + sub.pos_control.get_pos_error_z_cm()); } return 0; @@ -804,7 +804,7 @@ int16_t GCS_MAVLINK_Sub::high_latency_target_altitude() const uint8_t GCS_MAVLINK_Sub::high_latency_tgt_heading() const { // return units are deg/2 - if (sub.control_mode == AUTO || sub.control_mode == GUIDED) { + if (sub.control_mode == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) { // need to convert -18000->18000 to 0->360/2 return wrap_360_cd(sub.wp_nav.get_wp_bearing_to_destination()) / 200; } @@ -814,7 +814,7 @@ uint8_t GCS_MAVLINK_Sub::high_latency_tgt_heading() const uint16_t GCS_MAVLINK_Sub::high_latency_tgt_dist() const { // return units are dm - if (sub.control_mode == AUTO || sub.control_mode == GUIDED) { + if (sub.control_mode == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) { return MIN(sub.wp_nav.get_wp_distance_to_destination() * 0.001, UINT16_MAX); } return 0; @@ -823,7 +823,7 @@ uint16_t GCS_MAVLINK_Sub::high_latency_tgt_dist() const uint8_t GCS_MAVLINK_Sub::high_latency_tgt_airspeed() const { // return units are m/s*5 - if (sub.control_mode == AUTO || sub.control_mode == GUIDED) { + if (sub.control_mode == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) { return MIN((sub.pos_control.get_vel_desired_cms().length()/100) * 5, UINT8_MAX); } return 0; diff --git a/ArduSub/GCS_Sub.cpp b/ArduSub/GCS_Sub.cpp index d535a823e6..b57839c9bc 100644 --- a/ArduSub/GCS_Sub.cpp +++ b/ArduSub/GCS_Sub.cpp @@ -30,12 +30,12 @@ void GCS_Sub::update_vehicle_sensor_status_flags() MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; switch (sub.control_mode) { - case ALT_HOLD: - case AUTO: - case GUIDED: - case CIRCLE: - case SURFACE: - case POSHOLD: + case Mode::Number::ALT_HOLD: + case Mode::Number::AUTO: + case Mode::Number::GUIDED: + case Mode::Number::CIRCLE: + case Mode::Number::SURFACE: + case Mode::Number::POSHOLD: control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 1dbf759d6a..f2649bc841 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -278,7 +278,7 @@ const struct LogStructure Sub::log_structure[] = { void Sub::Log_Write_Vehicle_Startup_Messages() { // only 200(?) bytes are guaranteed by AP_Logger - logger.Write_Mode(control_mode, control_mode_reason); + logger.Write_Mode((uint8_t)control_mode, control_mode_reason); ahrs.Log_Write_Home_And_Origin(); gps.Write_AP_Logger_Log_Startup_messages(); } diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index bd73ce41a6..4275ddb710 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -25,7 +25,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); */ Sub::Sub() : logger(g.log_bitmask), - control_mode(MANUAL), + control_mode(Mode::Number::MANUAL), motors(MAIN_LOOP_RATE), auto_mode(Auto_WP), guided_mode(Guided_WP), @@ -37,7 +37,8 @@ Sub::Sub() wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control), loiter_nav(inertial_nav, ahrs_view, pos_control, attitude_control), circle_nav(inertial_nav, ahrs_view, pos_control), - param_loader(var_info) + param_loader(var_info), + flightmode(&mode_manual) { #if CONFIG_HAL_BOARD != HAL_BOARD_SITL failsafe.pilot_input = true; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 4dbf73b760..a9864acbc5 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -71,6 +71,7 @@ #include "Parameters.h" #include "AP_Arming_Sub.h" #include "GCS_Sub.h" +#include "mode.h" #include // Optical Flow library @@ -108,6 +109,17 @@ public: friend class ParametersG2; friend class AP_Arming_Sub; friend class RC_Channels_Sub; + friend class Mode; + friend class ModeManual; + friend class ModeStabilize; + friend class ModeAcro; + friend class ModeAlthold; + friend class ModeGuided; + friend class ModePoshold; + friend class ModeAuto; + friend class ModeCircle; + friend class ModeSurface; + friend class ModeMotordetect; Sub(void); @@ -190,9 +202,9 @@ private: // This is the state of the flight control system // There are multiple states defined such as STABILIZE, ACRO, - control_mode_t control_mode; + Mode::Number control_mode; - control_mode_t prev_control_mode; + Mode::Number prev_control_mode; #if RCMAP_ENABLED == ENABLED RCMapper rcmap; @@ -234,12 +246,6 @@ private: AP_Motors6DOF motors; - // Auto - AutoMode auto_mode; // controls which auto controller is run - - // Guided - GuidedMode guided_mode; // controls which controller is run (pos or vel) - // Circle bool circle_pilot_yaw_override; // true if pilot is overriding yaw @@ -416,61 +422,7 @@ private: bool verify_wait_delay(); bool verify_within_distance(); bool verify_yaw(); - bool acro_init(void); - void acro_run(); - void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out); - bool althold_init(void); - void althold_run(); - bool auto_init(void); - void auto_run(); - void auto_wp_start(const Vector3f& destination); - void auto_wp_start(const Location& dest_loc); - void auto_wp_run(); - void auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn); - void auto_circle_start(); - void auto_circle_run(); - void auto_nav_guided_start(); - void auto_nav_guided_run(); - bool auto_loiter_start(); - void auto_loiter_run(); - uint8_t get_default_auto_yaw_mode(bool rtl) const; - void set_auto_yaw_mode(uint8_t yaw_mode); - void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle); - void set_auto_yaw_roi(const Location &roi_location); - float get_auto_heading(void); - bool circle_init(void); - void circle_run(); - bool guided_init(bool ignore_checks = false); - void guided_pos_control_start(); - void guided_vel_control_start(); - void guided_posvel_control_start(); - void guided_angle_control_start(); - bool guided_set_destination(const Vector3f& destination); - bool guided_set_destination(const Location& dest_loc); - void guided_set_velocity(const Vector3f& velocity); - bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity); - void guided_set_angle(const Quaternion &q, float climb_rate_cms); - void guided_run(); - void guided_pos_control_run(); - void guided_vel_control_run(); - void guided_posvel_control_run(); - void guided_angle_control_run(); - void guided_limit_clear(); - void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm); - void guided_limit_init_time_and_pos(); - bool guided_limit_check(); - bool poshold_init(void); - void poshold_run(); - - bool motordetect_init(); - void motordetect_run(); - - bool stabilize_init(void); - void stabilize_run(); - void control_depth(); - bool manual_init(void); - void manual_run(); void failsafe_sensors_check(void); void failsafe_crash_check(); void failsafe_ekf_check(void); @@ -484,15 +436,12 @@ private: void mainloop_failsafe_enable(); void mainloop_failsafe_disable(); void fence_check(); - bool set_mode(control_mode_t mode, ModeReason reason); - bool set_mode(const uint8_t mode, const ModeReason reason) override; + bool set_mode(Mode::Number mode, ModeReason reason); + bool set_mode(const uint8_t new_mode, const ModeReason reason) override; uint8_t get_mode() const override { return (uint8_t)control_mode; } void update_flight_mode(); - void exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode); - bool mode_requires_GPS(control_mode_t mode); - bool mode_has_manual_throttle(control_mode_t mode); - bool mode_allows_arming(control_mode_t mode, bool arming_from_gcs); - void notify_flight_mode(control_mode_t mode); + void exit_mode(Mode::Number old_control_mode, Mode::Number new_control_mode); + void notify_flight_mode(); void read_inertia(); void update_surface_and_bottom_detector(); void set_surfaced(bool at_surface); @@ -563,8 +512,7 @@ private: void failsafe_internal_temperature_check(); void failsafe_terrain_act(void); - bool auto_terrain_recover_start(void); - void auto_terrain_recover_run(void); + void translate_wpnav_rp(float &lateral_out, float &forward_out); void translate_circle_nav_rp(float &lateral_out, float &forward_out); @@ -610,6 +558,24 @@ private: static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1, "_failsafe_priorities is missing the sentinel"); + Mode *mode_from_mode_num(const Mode::Number num); + void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode); + + Mode *flightmode; + ModeManual mode_manual; + ModeStabilize mode_stabilize; + ModeAcro mode_acro; + ModeAlthold mode_althold; + ModeAuto mode_auto; + ModeGuided mode_guided; + ModePoshold mode_poshold; + ModeCircle mode_circle; + ModeSurface mode_surface; + ModeMotordetect mode_motordetect; + + // Auto + AutoSubMode auto_mode; // controls which auto controller is run + GuidedSubMode guided_mode; public: void mainloop_failsafe_check(); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 855f8b0adf..f35eac7967 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -124,7 +124,7 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd) // called by mission library in mission.update() bool Sub::verify_command_callback(const AP_Mission::Mission_Command& cmd) { - if (control_mode == AUTO) { + if (control_mode == Mode::Number::AUTO) { bool cmd_complete = verify_command(cmd); // send message to GCS @@ -207,8 +207,8 @@ void Sub::exit_mission() AP_Notify::events.mission_complete = 1; // Try to enter loiter, if that fails, go to depth hold - if (!auto_loiter_start()) { - set_mode(ALT_HOLD, ModeReason::MISSION_END); + if (!mode_auto.auto_loiter_start()) { + set_mode(Mode::Number::ALT_HOLD, ModeReason::MISSION_END); } } @@ -243,7 +243,7 @@ void Sub::do_nav_wp(const AP_Mission::Mission_Command& cmd) loiter_time_max = cmd.p1; // Set wp navigation target - auto_wp_start(target_loc); + mode_auto.auto_wp_start(target_loc); } // do_surface - initiate surface procedure @@ -279,12 +279,12 @@ void Sub::do_surface(const AP_Mission::Mission_Command& cmd) } // Go to wp location - auto_wp_start(target_location); + mode_auto.auto_wp_start(target_location); } void Sub::do_RTL() { - auto_wp_start(ahrs.get_home()); + mode_auto.auto_wp_start(ahrs.get_home()); } // do_loiter_unlimited - start loitering with no end conditions @@ -323,7 +323,7 @@ void Sub::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) } // start way point navigator and provide it the desired location - auto_wp_start(target_loc); + mode_auto.auto_wp_start(target_loc); } // do_circle - initiate moving in a circle @@ -362,7 +362,7 @@ void Sub::do_circle(const AP_Mission::Mission_Command& cmd) const bool circle_direction_ccw = cmd.content.location.loiter_ccw; // move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge - auto_circle_movetoedge_start(circle_center, circle_radius_m, circle_direction_ccw); + mode_auto.auto_circle_movetoedge_start(circle_center, circle_radius_m, circle_direction_ccw); } // do_loiter_time - initiate loitering at a point for a given time period @@ -383,10 +383,10 @@ void Sub::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd) { if (cmd.p1 > 0) { // initialise guided limits - guided_limit_init_time_and_pos(); + mode_auto.guided_limit_init_time_and_pos(); // set navigation target - auto_nav_guided_start(); + mode_auto.auto_nav_guided_start(); } } #endif // NAV_GUIDED @@ -410,7 +410,7 @@ void Sub::do_nav_delay(const AP_Mission::Mission_Command& cmd) // do_guided_limits - pass guided limits to guided controller void Sub::do_guided_limits(const AP_Mission::Mission_Command& cmd) { - guided_limit_set(cmd.p1 * 1000, // convert seconds to ms + mode_guided.guided_limit_set(cmd.p1 * 1000, // convert seconds to ms cmd.content.guided_limits.alt_min * 100.0f, // convert meters to cm cmd.content.guided_limits.alt_max * 100.0f, // convert meters to cm cmd.content.guided_limits.horiz_max * 100.0f); // convert meters to cm @@ -459,7 +459,7 @@ bool Sub::verify_surface(const AP_Mission::Mission_Command& cmd) // TODO get xy target from current wp destination, because current location may be acceptance-radius away from original destination Location target_location(cmd.content.location.lat, cmd.content.location.lng, 0, Location::AltFrame::ABOVE_HOME); - auto_wp_start(target_location); + mode_auto.auto_wp_start(target_location); // advance to next state auto_surface_state = AUTO_SURFACE_STATE_ASCEND; @@ -529,13 +529,13 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd) } // start circling - auto_circle_start(); + mode_auto.auto_circle_start(); } return false; } // check if we have completed circling - return fabsf(circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1); + return fabsf(sub.circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1); } #if NAV_GUIDED == ENABLED @@ -548,7 +548,7 @@ bool Sub::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) } // check time and position limits - return guided_limit_check(); + return mode_auto.guided_limit_check(); } #endif // NAV_GUIDED @@ -579,7 +579,7 @@ void Sub::do_within_distance(const AP_Mission::Mission_Command& cmd) void Sub::do_yaw(const AP_Mission::Mission_Command& cmd) { - set_auto_yaw_look_at_heading( + sub.mode_auto.set_auto_yaw_look_at_heading( cmd.content.yaw.angle_deg, cmd.content.yaw.turn_rate_dps, cmd.content.yaw.direction, @@ -614,7 +614,7 @@ bool Sub::verify_yaw() { // set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command) if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) { - set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); + sub.mode_auto.set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); } // check if we are within 2 degrees of the target heading @@ -629,7 +629,7 @@ bool Sub::verify_yaw() bool Sub::do_guided(const AP_Mission::Mission_Command& cmd) { // only process guided waypoint if we are in guided mode - if (control_mode != GUIDED && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) { + if (control_mode != Mode::Number::GUIDED && !(control_mode == Mode::Number::AUTO && auto_mode == Auto_NavGuided)) { return false; } @@ -638,7 +638,7 @@ bool Sub::do_guided(const AP_Mission::Mission_Command& cmd) case MAV_CMD_NAV_WAYPOINT: { // set wp_nav's destination - return guided_set_destination(cmd.content.location); + return sub.mode_guided.guided_set_destination(cmd.content.location); } case MAV_CMD_CONDITION_YAW: @@ -681,7 +681,7 @@ void Sub::do_set_home(const AP_Mission::Mission_Command& cmd) // TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint void Sub::do_roi(const AP_Mission::Mission_Command& cmd) { - set_auto_yaw_roi(cmd.content.location); + sub.mode_auto.set_auto_yaw_roi(cmd.content.location); } // point the camera to a specified angle diff --git a/ArduSub/control_acro.cpp b/ArduSub/control_acro.cpp index 959ad69f8e..29b1e61891 100644 --- a/ArduSub/control_acro.cpp +++ b/ArduSub/control_acro.cpp @@ -1,34 +1,30 @@ #include "Sub.h" -/* - * control_acro.pde - init and run calls for acro flight mode - */ -// acro_init - initialise acro controller -bool Sub::acro_init() -{ +#include "Sub.h" + + +bool ModeAcro::init(bool ignore_checks) { // set target altitude to zero for reporting - pos_control.set_pos_target_z_cm(0); + position_control->set_pos_target_z_cm(0); // attitude hold inputs become thrust inputs in acro mode // set to neutral to prevent chaotic behavior (esp. roll/pitch) - set_neutral_controls(); + sub.set_neutral_controls(); return true; } -// acro_run - runs the acro controller -// should be called at 100hz or more -void Sub::acro_run() +void ModeAcro::run() { float target_roll, target_pitch, target_yaw; // if not armed set throttle to zero and exit immediately if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - attitude_control.set_throttle_out(0,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); return; } @@ -38,125 +34,13 @@ void Sub::acro_run() get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw); // run attitude controller - attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); + attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); // output pilot's throttle without angle boost - attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); + attitude_control->set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); //control_in is range 0-1000 //radio_in is raw pwm value motors.set_forward(channel_forward->norm_input()); motors.set_lateral(channel_lateral->norm_input()); } - - -// get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates -// returns desired angle rates in centi-degrees-per-second -void Sub::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out) -{ - float rate_limit; - Vector3f rate_ef_level, rate_bf_level, rate_bf_request; - - // apply circular limit to pitch and roll inputs - float total_in = norm(pitch_in, roll_in); - - if (total_in > ROLL_PITCH_INPUT_MAX) { - float ratio = (float)ROLL_PITCH_INPUT_MAX / total_in; - roll_in *= ratio; - pitch_in *= ratio; - } - - // calculate roll, pitch rate requests - if (g.acro_expo <= 0) { - rate_bf_request.x = roll_in * g.acro_rp_p; - rate_bf_request.y = pitch_in * g.acro_rp_p; - } else { - // expo variables - float rp_in, rp_in3, rp_out; - - // range check expo - if (g.acro_expo > 1.0f) { - g.acro_expo.set(1.0f); - } - - // roll expo - rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX; - rp_in3 = rp_in*rp_in*rp_in; - rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in); - rate_bf_request.x = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p; - - // pitch expo - rp_in = float(pitch_in)/ROLL_PITCH_INPUT_MAX; - rp_in3 = rp_in*rp_in*rp_in; - rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in); - rate_bf_request.y = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p; - } - - // calculate yaw rate request - rate_bf_request.z = yaw_in * g.acro_yaw_p; - - // calculate earth frame rate corrections to pull the vehicle back to level while in ACRO mode - - if (g.acro_trainer != ACRO_TRAINER_DISABLED) { - // Calculate trainer mode earth frame rate command for roll - int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); - rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll; - - // Calculate trainer mode earth frame rate command for pitch - int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor); - rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch; - - // Calculate trainer mode earth frame rate command for yaw - rate_ef_level.z = 0; - - // Calculate angle limiting earth frame rate commands - if (g.acro_trainer == ACRO_TRAINER_LIMITED) { - if (roll_angle > aparm.angle_max) { - rate_ef_level.x -= g.acro_balance_roll*(roll_angle-aparm.angle_max); - } else if (roll_angle < -aparm.angle_max) { - rate_ef_level.x -= g.acro_balance_roll*(roll_angle+aparm.angle_max); - } - - if (pitch_angle > aparm.angle_max) { - rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle-aparm.angle_max); - } else if (pitch_angle < -aparm.angle_max) { - rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle+aparm.angle_max); - } - } - - // convert earth-frame level rates to body-frame level rates - attitude_control.euler_rate_to_ang_vel(attitude_control.get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level); - - // combine earth frame rate corrections with rate requests - if (g.acro_trainer == ACRO_TRAINER_LIMITED) { - rate_bf_request.x += rate_bf_level.x; - rate_bf_request.y += rate_bf_level.y; - rate_bf_request.z += rate_bf_level.z; - } else { - float acro_level_mix = constrain_float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch(); - - // Scale leveling rates by stick input - rate_bf_level = rate_bf_level*acro_level_mix; - - // Calculate rate limit to prevent change of rate through inverted - rate_limit = fabsf(fabsf(rate_bf_request.x)-fabsf(rate_bf_level.x)); - rate_bf_request.x += rate_bf_level.x; - rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit); - - // Calculate rate limit to prevent change of rate through inverted - rate_limit = fabsf(fabsf(rate_bf_request.y)-fabsf(rate_bf_level.y)); - rate_bf_request.y += rate_bf_level.y; - rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit); - - // Calculate rate limit to prevent change of rate through inverted - rate_limit = fabsf(fabsf(rate_bf_request.z)-fabsf(rate_bf_level.z)); - rate_bf_request.z += rate_bf_level.z; - rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit); - } - } - - // hand back rate request - roll_out = rate_bf_request.x; - pitch_out = rate_bf_request.y; - yaw_out = rate_bf_request.z; -} diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index a420cfe314..3282054d73 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -1,46 +1,40 @@ #include "Sub.h" -/* - * control_althold.pde - init and run calls for althold, flight mode - */ - -// althold_init - initialise althold controller -bool Sub::althold_init() -{ - if(!control_check_barometer()) { +bool ModeAlthold::init(bool ignore_checks) { + if(!sub.control_check_barometer()) { return false; } // initialize vertical maximum speeds and acceleration // sets the maximum speed up and down returned by position controller - pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity - pos_control.init_z_controller(); + position_control->init_z_controller(); - last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading = ahrs.yaw_sensor; return true; } // althold_run - runs the althold controller // should be called at 100hz or more -void Sub::althold_run() +void ModeAlthold::run() { uint32_t tnow = AP_HAL::millis(); // initialize vertical speeds and acceleration - pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) - attitude_control.set_throttle_out(0.5,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); - pos_control.relax_z_controller(motors.get_throttle_hover()); - last_pilot_heading = ahrs.yaw_sensor; + attitude_control->set_throttle_out(0.5,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); + position_control->relax_z_controller(motors.get_throttle_hover()); + sub.last_pilot_heading = ahrs.yaw_sensor; return; } @@ -53,7 +47,7 @@ void Sub::althold_run() if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) { float target_yaw; Quaternion( - set_attitude_target_no_gps.packet.q + sub.set_attitude_target_no_gps.packet.q ).to_euler( target_roll, target_pitch, @@ -63,34 +57,34 @@ void Sub::althold_run() target_pitch = degrees(target_pitch); target_yaw = degrees(target_yaw); - attitude_control.input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true); + attitude_control->input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true); return; } - get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max_cd()); + sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->get_althold_lean_angle_max_cd()); // get pilot's desired yaw rate - float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // call attitude controller if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - last_pilot_heading = ahrs.yaw_sensor; - last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading } else { // hold current heading // this check is required to prevent bounce back after very fast yaw maneuvers // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped - if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading + if (tnow < sub.last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading target_yaw_rate = 0; // Stop rotation on yaw axis // call attitude controller with target yaw rate = 0 to decelerate on yaw axis - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - last_pilot_heading = ahrs.yaw_sensor; // update heading to hold + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold } else { // call attitude controller holding absolute absolute bearing - attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true); + attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, sub.last_pilot_heading, true); } } @@ -100,21 +94,21 @@ void Sub::althold_run() motors.set_lateral(channel_lateral->norm_input()); } -void Sub::control_depth() { - float target_climb_rate_cm_s = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); - target_climb_rate_cm_s = constrain_float(target_climb_rate_cm_s, -get_pilot_speed_dn(), g.pilot_speed_up); +void ModeAlthold::control_depth() { + float target_climb_rate_cm_s = sub.get_pilot_desired_climb_rate(channel_throttle->get_control_in()); + target_climb_rate_cm_s = constrain_float(target_climb_rate_cm_s, -sub.get_pilot_speed_dn(), g.pilot_speed_up); // desired_climb_rate returns 0 when within the deadzone. //we allow full control to the pilot, but as soon as there's no input, we handle being at surface/bottom if (fabsf(target_climb_rate_cm_s) < 0.05f) { - if (ap.at_surface) { - pos_control.set_pos_target_z_cm(MIN(pos_control.get_pos_target_z_cm(), g.surface_depth - 5.0f)); // set target to 5 cm below surface level - } else if (ap.at_bottom) { - pos_control.set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, pos_control.get_pos_target_z_cm())); // set target to 10 cm above bottom + if (sub.ap.at_surface) { + position_control->set_pos_target_z_cm(MIN(position_control->get_pos_target_z_cm(), g.surface_depth - 5.0f)); // set target to 5 cm below surface level + } else if (sub.ap.at_bottom) { + position_control->set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_target_z_cm())); // set target to 10 cm above bottom } } - pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s); - pos_control.update_z_controller(); + position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s); + position_control->update_z_controller(); } diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index 4631e8dcef..4370e1dfd8 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -7,42 +7,38 @@ * While in the auto flight mode, navigation or do/now commands can be run. * Code in this file implements the navigation commands */ - -// auto_init - initialise auto controller -bool Sub::auto_init() -{ - if (!position_ok() || mission.num_commands() < 2) { +bool ModeAuto::init(bool ignore_checks) { + if (!sub.position_ok() || sub.mission.num_commands() < 2) { return false; } - auto_mode = Auto_Loiter; + sub.auto_mode = Auto_Loiter; // stop ROI from carrying over from previous runs of the mission // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check - if (auto_yaw_mode == AUTO_YAW_ROI) { + if (sub.auto_yaw_mode == AUTO_YAW_ROI) { set_auto_yaw_mode(AUTO_YAW_HOLD); } // initialise waypoint controller - wp_nav.wp_and_spline_init(); + sub.wp_nav.wp_and_spline_init(); // clear guided limits guided_limit_clear(); // start/resume the mission (based on MIS_RESTART parameter) - mission.start_or_resume(); + sub.mission.start_or_resume(); return true; } // auto_run - runs the appropriate auto controller // according to the current auto_mode -// should be called at 100hz or more -void Sub::auto_run() +void ModeAuto::run() { - mission.update(); + sub.mission.update(); // call the correct auto controller - switch (auto_mode) { + switch (sub.auto_mode) { case Auto_WP: case Auto_CircleMoveToEdge: @@ -70,42 +66,42 @@ void Sub::auto_run() } // auto_wp_start - initialises waypoint controller to implement flying to a particular destination -void Sub::auto_wp_start(const Vector3f& destination) +void ModeAuto::auto_wp_start(const Vector3f& destination) { - auto_mode = Auto_WP; + sub.auto_mode = Auto_WP; // initialise wpnav (no need to check return status because terrain data is not used) - wp_nav.set_wp_destination(destination, false); + sub.wp_nav.set_wp_destination(destination, false); // initialise yaw // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI - if (auto_yaw_mode != AUTO_YAW_ROI) { + if (sub.auto_yaw_mode != AUTO_YAW_ROI) { set_auto_yaw_mode(get_default_auto_yaw_mode(false)); } } // auto_wp_start - initialises waypoint controller to implement flying to a particular destination -void Sub::auto_wp_start(const Location& dest_loc) +void ModeAuto::auto_wp_start(const Location& dest_loc) { - auto_mode = Auto_WP; + sub.auto_mode = Auto_WP; // send target to waypoint controller - if (!wp_nav.set_wp_destination_loc(dest_loc)) { + if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) { // failure to set destination can only be because of missing terrain data - failsafe_terrain_on_event(); + sub.failsafe_terrain_on_event(); return; } // initialise yaw // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI - if (auto_yaw_mode != AUTO_YAW_ROI) { + if (sub.auto_yaw_mode != AUTO_YAW_ROI) { set_auto_yaw_mode(get_default_auto_yaw_mode(false)); } } // auto_wp_run - runs the auto waypoint controller // called by auto_run at 100hz or more -void Sub::auto_wp_run() +void ModeAuto::auto_wp_run() { // if not armed set throttle to zero and exit immediately if (!motors.armed()) { @@ -114,17 +110,17 @@ void Sub::auto_wp_run() // call attitude controller // Sub vehicles do not stabilize roll/pitch/yaw when disarmed motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - attitude_control.set_throttle_out(0,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); - wp_nav.wp_and_spline_init(); // Reset xy target + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); + sub.wp_nav.wp_and_spline_init(); // Reset xy target return; } // process pilot's yaw input float target_yaw_rate = 0; - if (!failsafe.pilot_input) { + if (!sub.failsafe.pilot_input) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -137,13 +133,13 @@ void Sub::auto_wp_run() // TODO logic for terrain tracking target going below fence limit // TODO implement waypoint radius individually for each waypoint based on cmd.p2 // TODO fix auto yaw heading to switch to something appropriate when mission complete and switches to loiter - failsafe_terrain_set_status(wp_nav.update_wpnav()); + sub.failsafe_terrain_set_status(sub.wp_nav.update_wpnav()); /////////////////////// // update xy outputs // float lateral_out, forward_out; - translate_wpnav_rp(lateral_out, forward_out); + sub.translate_wpnav_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); @@ -151,52 +147,52 @@ void Sub::auto_wp_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - pos_control.update_z_controller(); + position_control->update_z_controller(); //////////////////////////// // update attitude output // // get pilot desired lean angles float target_roll, target_pitch; - get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); + sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max); // call attitude controller - if (auto_yaw_mode == AUTO_YAW_HOLD) { + if (sub.auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch & yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); } else { // roll, pitch from pilot, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true); + attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true); } } // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location -// we assume the caller has set the circle's circle with circle_nav.set_center() +// we assume the caller has set the circle's circle with sub.circle_nav.set_center() // we assume the caller has performed all required GPS_ok checks -void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn) +void ModeAuto::auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn) { // set circle center - circle_nav.set_center(circle_center); + sub.circle_nav.set_center(circle_center); // set circle radius if (!is_zero(radius_m)) { - circle_nav.set_radius_cm(radius_m * 100.0f); + sub.circle_nav.set_radius_cm(radius_m * 100.0f); } // set circle direction by using rate - float current_rate = circle_nav.get_rate(); + float current_rate = sub.circle_nav.get_rate(); current_rate = ccw_turn ? -fabsf(current_rate) : fabsf(current_rate); - circle_nav.set_rate(current_rate); + sub.circle_nav.set_rate(current_rate); // check our distance from edge of circle Vector3f circle_edge_neu; - circle_nav.get_closest_point_on_circle(circle_edge_neu); + sub.circle_nav.get_closest_point_on_circle(circle_edge_neu); float dist_to_edge = (inertial_nav.get_position_neu_cm() - circle_edge_neu).length(); // if more than 3m then fly to edge if (dist_to_edge > 300.0f) { // set the state to move to the edge of the circle - auto_mode = Auto_CircleMoveToEdge; + sub.auto_mode = Auto_CircleMoveToEdge; // convert circle_edge_neu to Location Location circle_edge(circle_edge_neu, Location::AltFrame::ABOVE_ORIGIN); @@ -205,14 +201,14 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame()); // initialise wpnav to move to edge of circle - if (!wp_nav.set_wp_destination_loc(circle_edge)) { + if (!sub.wp_nav.set_wp_destination_loc(circle_edge)) { // failure to set destination can only be because of missing terrain data - failsafe_terrain_on_event(); + sub.failsafe_terrain_on_event(); } // if we are outside the circle, point at the edge, otherwise hold yaw - float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy_cm().topostype(), circle_nav.get_center().xy()); - if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) { + float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy_cm().topostype(), sub.circle_nav.get_center().xy()); + if (dist_to_center > sub.circle_nav.get_radius() && dist_to_center > 500) { set_auto_yaw_mode(get_default_auto_yaw_mode(false)); } else { // vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle @@ -225,23 +221,23 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi // auto_circle_start - initialises controller to fly a circle in AUTO flight mode // assumes that circle_nav object has already been initialised with circle center and radius -void Sub::auto_circle_start() +void ModeAuto::auto_circle_start() { - auto_mode = Auto_Circle; + sub.auto_mode = Auto_Circle; // initialise circle controller - circle_nav.init(circle_nav.get_center(), circle_nav.center_is_terrain_alt(), circle_nav.get_rate()); + sub.circle_nav.init(sub.circle_nav.get_center(), sub.circle_nav.center_is_terrain_alt(), sub.circle_nav.get_rate()); } // auto_circle_run - circle in AUTO flight mode // called by auto_run at 100hz or more -void Sub::auto_circle_run() +void ModeAuto::auto_circle_run() { // call circle controller - failsafe_terrain_set_status(circle_nav.update()); + sub.failsafe_terrain_set_status(sub.circle_nav.update()); float lateral_out, forward_out; - translate_circle_nav_rp(lateral_out, forward_out); + sub.translate_circle_nav_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); @@ -249,50 +245,47 @@ void Sub::auto_circle_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - pos_control.update_z_controller(); + position_control->update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true); + attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), sub.circle_nav.get_yaw(), true); } #if NAV_GUIDED == ENABLED // auto_nav_guided_start - hand over control to external navigation controller in AUTO mode -void Sub::auto_nav_guided_start() +void ModeAuto::auto_nav_guided_start() { - auto_mode = Auto_NavGuided; - - // call regular guided flight mode initialisation - guided_init(true); - + sub.mode_guided.init(true); + sub.auto_mode = Auto_NavGuided; // initialise guided start time and position as reference for limit checking - guided_limit_init_time_and_pos(); + sub.mode_auto.guided_limit_init_time_and_pos(); } // auto_nav_guided_run - allows control by external navigation controller // called by auto_run at 100hz or more -void Sub::auto_nav_guided_run() +void ModeAuto::auto_nav_guided_run() { // call regular guided flight mode run function - guided_run(); + sub.mode_guided.run(); } #endif // NAV_GUIDED // auto_loiter_start - initialises loitering in auto mode // returns success/failure because this can be called by exit_mission -bool Sub::auto_loiter_start() +bool ModeAuto::auto_loiter_start() { // return failure if GPS is bad - if (!position_ok()) { + if (!sub.position_ok()) { return false; } - auto_mode = Auto_Loiter; + sub.auto_mode = Auto_Loiter; // calculate stopping point Vector3f stopping_point; - wp_nav.get_wp_stopping_point(stopping_point); + sub.wp_nav.get_wp_stopping_point(stopping_point); // initialise waypoint controller target to stopping point - wp_nav.set_wp_destination(stopping_point); + sub.wp_nav.set_wp_destination(stopping_point); // hold yaw at current heading set_auto_yaw_mode(AUTO_YAW_HOLD); @@ -302,35 +295,35 @@ bool Sub::auto_loiter_start() // auto_loiter_run - loiter in AUTO flight mode // called by auto_run at 100hz or more -void Sub::auto_loiter_run() +void ModeAuto::auto_loiter_run() { // if not armed set throttle to zero and exit immediately if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out(0,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); - wp_nav.wp_and_spline_init(); // Reset xy target + sub.wp_nav.wp_and_spline_init(); // Reset xy target return; } // accept pilot input of yaw float target_yaw_rate = 0; - if (!failsafe.pilot_input) { - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + if (!sub.failsafe.pilot_input) { + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } // set motors to full range motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run waypoint and z-axis position controller - failsafe_terrain_set_status(wp_nav.update_wpnav()); + sub.failsafe_terrain_set_status(sub.wp_nav.update_wpnav()); /////////////////////// // update xy outputs // float lateral_out, forward_out; - translate_wpnav_rp(lateral_out, forward_out); + sub.translate_wpnav_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); @@ -338,102 +331,33 @@ void Sub::auto_loiter_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - pos_control.update_z_controller(); + position_control->update_z_controller(); // get pilot desired lean angles float target_roll, target_pitch; - get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); + sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max); // roll & pitch & yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); } -// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter -// set rtl parameter to true if this is during an RTL -uint8_t Sub::get_default_auto_yaw_mode(bool rtl) const -{ - switch (g.wp_yaw_behavior) { - - case WP_YAW_BEHAVIOR_NONE: - return AUTO_YAW_HOLD; - break; - - case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL: - if (rtl) { - return AUTO_YAW_HOLD; - } else { - return AUTO_YAW_LOOK_AT_NEXT_WP; - } - break; - - case WP_YAW_BEHAVIOR_LOOK_AHEAD: - return AUTO_YAW_LOOK_AHEAD; - break; - - case WP_YAW_BEHAVIOR_CORRECT_XTRACK: - return AUTO_YAW_CORRECT_XTRACK; - break; - - case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP: - default: - return AUTO_YAW_LOOK_AT_NEXT_WP; - break; - } -} - -// set_auto_yaw_mode - sets the yaw mode for auto -void Sub::set_auto_yaw_mode(uint8_t yaw_mode) -{ - // return immediately if no change - if (auto_yaw_mode == yaw_mode) { - return; - } - auto_yaw_mode = yaw_mode; - - // perform initialisation - switch (auto_yaw_mode) { - - case AUTO_YAW_LOOK_AT_NEXT_WP: - // wpnav will initialise heading when wpnav's set_destination method is called - break; - - case AUTO_YAW_ROI: - // point towards a location held in yaw_look_at_WP - yaw_look_at_WP_bearing = ahrs.yaw_sensor; - break; - - case AUTO_YAW_LOOK_AT_HEADING: - // keep heading pointing in the direction held in yaw_look_at_heading - // caller should set the yaw_look_at_heading - break; - - case AUTO_YAW_LOOK_AHEAD: - // Commanded Yaw to automatically look ahead. - yaw_look_ahead_bearing = ahrs.yaw_sensor; - break; - - case AUTO_YAW_RESETTOARMEDYAW: - // initial_armed_bearing will be set during arming so no init required - break; - } -} // set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode -void Sub::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle) +void ModeAuto::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle) { // get current yaw target - int32_t curr_yaw_target = attitude_control.get_att_target_euler_cd().z; + int32_t curr_yaw_target = attitude_control->get_att_target_euler_cd().z; // get final angle, 1 = Relative, 0 = Absolute if (relative_angle == 0) { // absolute angle - yaw_look_at_heading = wrap_360_cd(angle_deg * 100); + sub.yaw_look_at_heading = wrap_360_cd(angle_deg * 100); } else { // relative angle if (direction < 0) { angle_deg = -angle_deg; } - yaw_look_at_heading = wrap_360_cd((angle_deg*100+curr_yaw_target)); + sub.yaw_look_at_heading = wrap_360_cd((angle_deg*100+curr_yaw_target)); } // get turn speed @@ -441,10 +365,10 @@ void Sub::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int // see AP_Float _slew_yaw in AC_AttitudeControl if (is_zero(turn_rate_dps)) { // default to regular auto slew rate - yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE; + sub.yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE; } else { - int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps; - yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec + int32_t turn_rate = (wrap_180_cd(sub.yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps; + sub.yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec } // set yaw mode @@ -454,7 +378,7 @@ void Sub::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int } // set_auto_yaw_roi - sets the yaw to look at roi for auto mode -void Sub::set_auto_yaw_roi(const Location &roi_location) +void ModeAuto::set_auto_yaw_roi(const Location &roi_location) { // if location is zero lat, lon and altitude turn off ROI if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) { @@ -462,20 +386,20 @@ void Sub::set_auto_yaw_roi(const Location &roi_location) set_auto_yaw_mode(get_default_auto_yaw_mode(false)); #if HAL_MOUNT_ENABLED // switch off the camera tracking if enabled - if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { - camera_mount.set_mode_to_default(); + if (sub.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { + sub.camera_mount.set_mode_to_default(); } #endif // HAL_MOUNT_ENABLED } else { #if HAL_MOUNT_ENABLED // check if mount type requires us to rotate the sub - if (!camera_mount.has_pan_control()) { - if (roi_location.get_vector_from_origin_NEU(roi_WP)) { + if (!sub.camera_mount.has_pan_control()) { + if (roi_location.get_vector_from_origin_NEU(sub.roi_WP)) { set_auto_yaw_mode(AUTO_YAW_ROI); } } // send the command to the camera mount - camera_mount.set_roi_target(roi_location); + sub.camera_mount.set_roi_target(roi_location); // TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below) // 0: do nothing @@ -485,70 +409,18 @@ void Sub::set_auto_yaw_roi(const Location &roi_location) // 4: point at a target given a target id (can't be implemented) #else // if we have no camera mount aim the sub at the location - if (roi_location.get_vector_from_origin_NEU(roi_WP)) { + if (roi_location.get_vector_from_origin_NEU(sub.roi_WP)) { set_auto_yaw_mode(AUTO_YAW_ROI); } #endif // HAL_MOUNT_ENABLED } } -// get_auto_heading - returns target heading depending upon auto_yaw_mode -// 100hz update rate -float Sub::get_auto_heading() -{ - switch (auto_yaw_mode) { - - case AUTO_YAW_ROI: - // point towards a location held in roi_WP - return get_roi_yaw(); - break; - - case AUTO_YAW_LOOK_AT_HEADING: - // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed - return yaw_look_at_heading; - break; - - case AUTO_YAW_LOOK_AHEAD: - // Commanded Yaw to automatically look ahead. - return get_look_ahead_yaw(); - break; - - case AUTO_YAW_RESETTOARMEDYAW: - // changes yaw to be same as when quad was armed - return initial_armed_bearing; - break; - - case AUTO_YAW_CORRECT_XTRACK: { - // TODO return current yaw if not in appropriate mode - // Bearing of current track (centidegrees) - float track_bearing = get_bearing_cd(wp_nav.get_wp_origin().xy(), wp_nav.get_wp_destination().xy()); - - // Bearing from current position towards intermediate position target (centidegrees) - const Vector2f target_vel_xy{pos_control.get_vel_target_cms().x, pos_control.get_vel_target_cms().y}; - float angle_error = 0.0f; - if (target_vel_xy.length() >= pos_control.get_max_speed_xy_cms() * 0.1f) { - const float desired_angle_cd = degrees(target_vel_xy.angle()) * 100.0f; - angle_error = wrap_180_cd(desired_angle_cd - track_bearing); - } - float angle_limited = constrain_float(angle_error, -g.xtrack_angle_limit * 100.0f, g.xtrack_angle_limit * 100.0f); - return wrap_360_cd(track_bearing + angle_limited); - } - break; - - case AUTO_YAW_LOOK_AT_NEXT_WP: - default: - // point towards next waypoint. - // we don't use wp_bearing because we don't want the vehicle to turn too much during flight - return wp_nav.get_yaw(); - break; - } -} - // Return true if it is possible to recover from a rangefinder failure -bool Sub::auto_terrain_recover_start() +bool ModeAuto::auto_terrain_recover_start() { // Check rangefinder status to see if recovery is possible - switch (rangefinder.status_orient(ROTATION_PITCH_270)) { + switch (sub.rangefinder.status_orient(ROTATION_PITCH_270)) { case RangeFinder::Status::OutOfRangeLow: case RangeFinder::Status::OutOfRangeHigh: @@ -556,7 +428,7 @@ bool Sub::auto_terrain_recover_start() // RangeFinder::Good if just one valid sample was obtained recently, but ::rangefinder_state.alt_healthy // requires several consecutive valid readings for wpnav to accept rangefinder data case RangeFinder::Status::Good: - auto_mode = Auto_TerrainRecover; + sub.auto_mode = Auto_TerrainRecover; break; // Not connected or no data @@ -565,21 +437,21 @@ bool Sub::auto_terrain_recover_start() } // Initialize recovery timeout time - fs_terrain_recover_start_ms = AP_HAL::millis(); + sub.fs_terrain_recover_start_ms = AP_HAL::millis(); // Stop mission - mission.stop(); + sub.mission.stop(); // Reset xy target - loiter_nav.clear_pilot_desired_acceleration(); - loiter_nav.init_target(); + sub.loiter_nav.clear_pilot_desired_acceleration(); + sub.loiter_nav.init_target(); // Reset z axis controller - pos_control.relax_z_controller(motors.get_throttle_hover()); + position_control->relax_z_controller(motors.get_throttle_hover()); // initialize vertical maximum speeds and acceleration - pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); - pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); + position_control->set_max_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z()); + position_control->set_correction_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z()); gcs().send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery"); return true; @@ -588,7 +460,7 @@ bool Sub::auto_terrain_recover_start() // Attempt recovery from terrain failsafe // If recovery is successful resume mission // If recovery fails revert to failsafe action -void Sub::auto_terrain_recover_run() +void ModeAuto::auto_terrain_recover_run() { float target_climb_rate = 0; static uint32_t rangefinder_recovery_ms = 0; @@ -596,23 +468,23 @@ void Sub::auto_terrain_recover_run() // if not armed set throttle to zero and exit immediately if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - attitude_control.set_throttle_out(0,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); - loiter_nav.init_target(); // Reset xy target - pos_control.relax_z_controller(motors.get_throttle_hover()); // Reset z axis controller + sub.loiter_nav.init_target(); // Reset xy target + position_control->relax_z_controller(motors.get_throttle_hover()); // Reset z axis controller return; } - switch (rangefinder.status_orient(ROTATION_PITCH_270)) { + switch (sub.rangefinder.status_orient(ROTATION_PITCH_270)) { case RangeFinder::Status::OutOfRangeLow: - target_climb_rate = wp_nav.get_default_speed_up(); + target_climb_rate = sub.wp_nav.get_default_speed_up(); rangefinder_recovery_ms = 0; break; case RangeFinder::Status::OutOfRangeHigh: - target_climb_rate = wp_nav.get_default_speed_down(); + target_climb_rate = sub.wp_nav.get_default_speed_down(); rangefinder_recovery_ms = 0; break; @@ -620,21 +492,21 @@ void Sub::auto_terrain_recover_run() target_climb_rate = 0; // Attempt to hold current depth - if (rangefinder_state.alt_healthy) { + if (sub.rangefinder_state.alt_healthy) { // Start timer as soon as rangefinder is healthy if (rangefinder_recovery_ms == 0) { rangefinder_recovery_ms = AP_HAL::millis(); - pos_control.relax_z_controller(motors.get_throttle_hover()); // Reset alt hold targets + position_control->relax_z_controller(motors.get_throttle_hover()); // Reset alt hold targets } // 1.5 seconds of healthy rangefinder means we can resume mission with terrain enabled if (AP_HAL::millis() > rangefinder_recovery_ms + 1500) { gcs().send_text(MAV_SEVERITY_INFO, "Terrain failsafe recovery successful!"); - failsafe_terrain_set_status(true); // Reset failsafe timers - failsafe.terrain = false; // Clear flag - auto_mode = Auto_Loiter; // Switch back to loiter for next iteration - mission.resume(); // Resume mission + sub.failsafe_terrain_set_status(true); // Reset failsafe timers + sub.failsafe.terrain = false; // Clear flag + sub.auto_mode = Auto_Loiter; // Switch back to loiter for next iteration + sub.mission.resume(); // Resume mission rangefinder_recovery_ms = 0; // Reset for subsequent recoveries } @@ -646,25 +518,25 @@ void Sub::auto_terrain_recover_run() // Terrain failsafe recovery has failed, terrain data is not available // and rangefinder is not connected, or has stopped responding gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery failure: No Rangefinder!"); - failsafe_terrain_act(); + sub.failsafe_terrain_act(); rangefinder_recovery_ms = 0; return; } // exit on failure (timeout) - if (AP_HAL::millis() > fs_terrain_recover_start_ms + FS_TERRAIN_RECOVER_TIMEOUT_MS) { + if (AP_HAL::millis() > sub.fs_terrain_recover_start_ms + FS_TERRAIN_RECOVER_TIMEOUT_MS) { // Recovery has failed, revert to failsafe action gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery timeout!"); - failsafe_terrain_act(); + sub.failsafe_terrain_act(); } // run loiter controller - loiter_nav.update(); + sub.loiter_nav.update(); /////////////////////// // update xy targets // float lateral_out, forward_out; - translate_wpnav_rp(lateral_out, forward_out); + sub.translate_wpnav_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); @@ -672,8 +544,8 @@ void Sub::auto_terrain_recover_run() ///////////////////// // update z target // - pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate); - pos_control.update_z_controller(); + position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); + position_control->update_z_controller(); //////////////////////////// // update angular targets // @@ -681,11 +553,11 @@ void Sub::auto_terrain_recover_run() float target_pitch = 0; // convert pilot input to lean angles - // To-Do: convert get_pilot_desired_lean_angles to return angles as floats - get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); + // To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats + sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max); float target_yaw_rate = 0; // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); } diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp index fca637e9bf..28be79dbad 100644 --- a/ArduSub/control_circle.cpp +++ b/ArduSub/control_circle.cpp @@ -5,82 +5,82 @@ */ // circle_init - initialise circle controller flight mode -bool Sub::circle_init() +bool ModeCircle::init(bool ignore_checks) { - if (!position_ok()) { + if (!sub.position_ok()) { return false; } - circle_pilot_yaw_override = false; + sub.circle_pilot_yaw_override = false; // initialize speeds and accelerations - pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); - pos_control.set_correction_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); - pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->set_max_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration()); + position_control->set_correction_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration()); + position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise circle controller including setting the circle center based on vehicle speed - circle_nav.init(); + sub.circle_nav.init(); return true; } // circle_run - runs the circle flight mode // should be called at 100hz or more -void Sub::circle_run() +void ModeCircle::run() { float target_yaw_rate = 0; float target_climb_rate = 0; // update parameters, to allow changing at runtime - pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); - pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->set_max_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration()); + position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // if not armed set throttle to zero and exit immediately if (!motors.armed()) { // To-Do: add some initialisation of position controllers motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out(0,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); - circle_nav.init(); + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); + sub.circle_nav.init(); return; } // process pilot inputs // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { - circle_pilot_yaw_override = true; + sub.circle_pilot_yaw_override = true; } // get pilot desired climb rate - target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); + target_climb_rate = sub.get_pilot_desired_climb_rate(channel_throttle->get_control_in()); // set motors to full range motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run circle controller - failsafe_terrain_set_status(circle_nav.update()); + sub.failsafe_terrain_set_status(sub.circle_nav.update()); /////////////////////// // update xy outputs // float lateral_out, forward_out; - translate_circle_nav_rp(lateral_out, forward_out); + sub.translate_circle_nav_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); motors.set_forward(forward_out); // call attitude controller - if (circle_pilot_yaw_override) { - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + if (sub.circle_pilot_yaw_override) { + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); } else { - attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true); + attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), sub.circle_nav.get_yaw(), true); } // update altitude target and call position controller - pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate); - pos_control.update_z_controller(); + position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); + position_control->update_z_controller(); } diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index 3c13e04b23..4c65cca42c 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -29,9 +29,9 @@ struct Guided_Limit { } guided_limit; // guided_init - initialise guided controller -bool Sub::guided_init(bool ignore_checks) +bool ModeGuided::init(bool ignore_checks) { - if (!position_ok() && !ignore_checks) { + if (!sub.position_ok() && !ignore_checks) { return false; } @@ -40,73 +40,107 @@ bool Sub::guided_init(bool ignore_checks) return true; } +// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter +// set rtl parameter to true if this is during an RTL +autopilot_yaw_mode ModeGuided::get_default_auto_yaw_mode(bool rtl) const +{ + switch (g.wp_yaw_behavior) { + + case WP_YAW_BEHAVIOR_NONE: + return AUTO_YAW_HOLD; + break; + + case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL: + if (rtl) { + return AUTO_YAW_HOLD; + } else { + return AUTO_YAW_LOOK_AT_NEXT_WP; + } + break; + + case WP_YAW_BEHAVIOR_LOOK_AHEAD: + return AUTO_YAW_LOOK_AHEAD; + break; + + case WP_YAW_BEHAVIOR_CORRECT_XTRACK: + return AUTO_YAW_CORRECT_XTRACK; + break; + + case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP: + default: + return AUTO_YAW_LOOK_AT_NEXT_WP; + break; + } +} + + // initialise guided mode's position controller -void Sub::guided_pos_control_start() +void ModeGuided::guided_pos_control_start() { // set to position control mode - guided_mode = Guided_WP; + sub.guided_mode = Guided_WP; // initialise waypoint controller - wp_nav.wp_and_spline_init(); + sub.wp_nav.wp_and_spline_init(); // initialise wpnav to stopping point at current altitude // To-Do: set to current location if disarmed? // To-Do: set to stopping point altitude? Vector3f stopping_point; - wp_nav.get_wp_stopping_point(stopping_point); + sub.wp_nav.get_wp_stopping_point(stopping_point); // no need to check return status because terrain data is not used - wp_nav.set_wp_destination(stopping_point, false); + sub.wp_nav.set_wp_destination(stopping_point, false); // initialise yaw set_auto_yaw_mode(get_default_auto_yaw_mode(false)); } // initialise guided mode's velocity controller -void Sub::guided_vel_control_start() +void ModeGuided::guided_vel_control_start() { // set guided_mode to velocity controller - guided_mode = Guided_Velocity; + sub.guided_mode = Guided_Velocity; // initialize vertical maximum speeds and acceleration - pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise velocity controller - pos_control.init_z_controller(); - pos_control.init_xy_controller(); + position_control->init_z_controller(); + position_control->init_xy_controller(); } // initialise guided mode's posvel controller -void Sub::guided_posvel_control_start() +void ModeGuided::guided_posvel_control_start() { // set guided_mode to velocity controller - guided_mode = Guided_PosVel; + sub.guided_mode = Guided_PosVel; // set vertical speed and acceleration - pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); - pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); + position_control->set_max_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z()); + position_control->set_correction_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z()); // initialise velocity controller - pos_control.init_z_controller(); - pos_control.init_xy_controller(); + position_control->init_z_controller(); + position_control->init_xy_controller(); // pilot always controls yaw set_auto_yaw_mode(AUTO_YAW_HOLD); } // initialise guided mode's angle controller -void Sub::guided_angle_control_start() +void ModeGuided::guided_angle_control_start() { // set guided_mode to velocity controller - guided_mode = Guided_Angle; + sub.guided_mode = Guided_Angle; // set vertical speed and acceleration - pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); - pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); + position_control->set_max_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z()); + position_control->set_correction_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z()); // initialise velocity controller - pos_control.init_z_controller(); + position_control->init_z_controller(); // initialise targets guided_angle_state.update_time_ms = AP_HAL::millis(); @@ -122,17 +156,17 @@ void Sub::guided_angle_control_start() // guided_set_destination - sets guided mode's target destination // Returns true if the fence is enabled and guided waypoint is within the fence // else return false if the waypoint is outside the fence -bool Sub::guided_set_destination(const Vector3f& destination) +bool ModeGuided::guided_set_destination(const Vector3f& destination) { // ensure we are in position control mode - if (guided_mode != Guided_WP) { + if (sub.guided_mode != Guided_WP) { guided_pos_control_start(); } #if AP_FENCE_ENABLED // reject destination if outside the fence const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); - if (!fence.check_destination_within_fence(dest_loc)) { + if (!sub.fence.check_destination_within_fence(dest_loc)) { AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; @@ -140,34 +174,34 @@ bool Sub::guided_set_destination(const Vector3f& destination) #endif // no need to check return status because terrain data is not used - wp_nav.set_wp_destination(destination, false); + sub.wp_nav.set_wp_destination(destination, false); // log target - Log_Write_GuidedTarget(guided_mode, destination, Vector3f()); + sub.Log_Write_GuidedTarget(sub.guided_mode, destination, Vector3f()); return true; } // sets guided mode's target from a Location object // returns false if destination could not be set (probably caused by missing terrain data) // or if the fence is enabled and guided waypoint is outside the fence -bool Sub::guided_set_destination(const Location& dest_loc) +bool ModeGuided::guided_set_destination(const Location& dest_loc) { // ensure we are in position control mode - if (guided_mode != Guided_WP) { + if (sub.guided_mode != Guided_WP) { guided_pos_control_start(); } #if AP_FENCE_ENABLED // reject destination outside the fence. // Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails - if (!fence.check_destination_within_fence(dest_loc)) { + if (!sub.fence.check_destination_within_fence(dest_loc)) { AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } #endif - if (!wp_nav.set_wp_destination_loc(dest_loc)) { + if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) { // failure to set destination can only be because of missing terrain data AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); // failure is propagated to GCS with NAK @@ -175,36 +209,36 @@ bool Sub::guided_set_destination(const Location& dest_loc) } // log target - Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f()); + sub.Log_Write_GuidedTarget(sub.guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f()); return true; } // guided_set_velocity - sets guided mode's target velocity -void Sub::guided_set_velocity(const Vector3f& velocity) +void ModeGuided::guided_set_velocity(const Vector3f& velocity) { // check we are in velocity control mode - if (guided_mode != Guided_Velocity) { + if (sub.guided_mode != Guided_Velocity) { guided_vel_control_start(); } update_time_ms = AP_HAL::millis(); // set position controller velocity target - pos_control.set_vel_desired_cms(velocity); + position_control->set_vel_desired_cms(velocity); } // set guided mode posvel target -bool Sub::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity) +bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity) { // check we are in velocity control mode - if (guided_mode != Guided_PosVel) { + if (sub.guided_mode != Guided_PosVel) { guided_posvel_control_start(); } #if AP_FENCE_ENABLED // reject destination if outside the fence const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); - if (!fence.check_destination_within_fence(dest_loc)) { + if (!sub.fence.check_destination_within_fence(dest_loc)) { AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; @@ -215,21 +249,21 @@ bool Sub::guided_set_destination_posvel(const Vector3f& destination, const Vecto posvel_pos_target_cm = destination.topostype(); posvel_vel_target_cms = velocity; - pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f()); + position_control->input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f()); float dz = posvel_pos_target_cm.z; - pos_control.input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0); + position_control->input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0); posvel_pos_target_cm.z = dz; // log target - Log_Write_GuidedTarget(guided_mode, destination, velocity); + sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity); return true; } // set guided mode angle target -void Sub::guided_set_angle(const Quaternion &q, float climb_rate_cms) +void ModeGuided::guided_set_angle(const Quaternion &q, float climb_rate_cms) { // check we are in velocity control mode - if (guided_mode != Guided_Angle) { + if (sub.guided_mode != Guided_Angle) { guided_angle_control_start(); } @@ -245,10 +279,10 @@ void Sub::guided_set_angle(const Quaternion &q, float climb_rate_cms) // guided_run - runs the guided controller // should be called at 100hz or more -void Sub::guided_run() +void ModeGuided::run() { // call the correct auto controller - switch (guided_mode) { + switch (sub.guided_mode) { case Guided_WP: // run position controller @@ -274,23 +308,23 @@ void Sub::guided_run() // guided_pos_control_run - runs the guided position controller // called from guided_run -void Sub::guided_pos_control_run() +void ModeGuided::guided_pos_control_run() { // if motors not enabled set throttle to zero and exit immediately if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out(0,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); - wp_nav.wp_and_spline_init(); + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); + sub.wp_nav.wp_and_spline_init(); return; } // process pilot's yaw input float target_yaw_rate = 0; - if (!failsafe.pilot_input) { + if (!sub.failsafe.pilot_input) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -300,10 +334,10 @@ void Sub::guided_pos_control_run() motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run waypoint controller - failsafe_terrain_set_status(wp_nav.update_wpnav()); + sub.failsafe_terrain_set_status(sub.wp_nav.update_wpnav()); float lateral_out, forward_out; - translate_wpnav_rp(lateral_out, forward_out); + sub.translate_wpnav_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); @@ -311,39 +345,39 @@ void Sub::guided_pos_control_run() // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle - pos_control.update_z_controller(); + position_control->update_z_controller(); // call attitude controller - if (auto_yaw_mode == AUTO_YAW_HOLD) { + if (sub.auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch & yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); } else { // roll, pitch from pilot, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); + attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); } } // guided_vel_control_run - runs the guided velocity controller // called from guided_run -void Sub::guided_vel_control_run() +void ModeGuided::guided_vel_control_run() { // ifmotors not enabled set throttle to zero and exit immediately if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out(0,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); // initialise velocity controller - pos_control.init_z_controller(); - pos_control.init_xy_controller(); + position_control->init_z_controller(); + position_control->init_xy_controller(); return; } // process pilot's yaw input float target_yaw_rate = 0; - if (!failsafe.pilot_input) { + if (!sub.failsafe.pilot_input) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -354,54 +388,54 @@ void Sub::guided_vel_control_run() // set velocity to zero if no updates received for 3 seconds uint32_t tnow = AP_HAL::millis(); - if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_vel_desired_cms().is_zero()) { - pos_control.set_vel_desired_cms(Vector3f(0,0,0)); + if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !position_control->get_vel_desired_cms().is_zero()) { + position_control->set_vel_desired_cms(Vector3f(0,0,0)); } - pos_control.stop_pos_xy_stabilisation(); + position_control->stop_pos_xy_stabilisation(); // call velocity controller which includes z axis controller - pos_control.update_xy_controller(); - pos_control.update_z_controller(); + position_control->update_xy_controller(); + position_control->update_z_controller(); float lateral_out, forward_out; - translate_pos_control_rp(lateral_out, forward_out); + sub.translate_pos_control_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); motors.set_forward(forward_out); // call attitude controller - if (auto_yaw_mode == AUTO_YAW_HOLD) { + if (sub.auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch & yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); } else { // roll, pitch from pilot, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); + attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); } } // guided_posvel_control_run - runs the guided posvel controller // called from guided_run -void Sub::guided_posvel_control_run() +void ModeGuided::guided_posvel_control_run() { // if motors not enabled set throttle to zero and exit immediately if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out(0,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); // initialise velocity controller - pos_control.init_z_controller(); - pos_control.init_xy_controller(); + position_control->init_z_controller(); + position_control->init_xy_controller(); return; } // process pilot's yaw input float target_yaw_rate = 0; - if (!failsafe.pilot_input) { + if (!sub.failsafe.pilot_input) { // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -417,47 +451,47 @@ void Sub::guided_posvel_control_run() } // advance position target using velocity target - posvel_pos_target_cm += (posvel_vel_target_cms * pos_control.get_dt()).topostype(); + posvel_pos_target_cm += (posvel_vel_target_cms * position_control->get_dt()).topostype(); // send position and velocity targets to position controller - pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f()); + position_control->input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f()); float pz = posvel_pos_target_cm.z; - pos_control.input_pos_vel_accel_z(pz, posvel_vel_target_cms.z, 0); + position_control->input_pos_vel_accel_z(pz, posvel_vel_target_cms.z, 0); posvel_pos_target_cm.z = pz; // run position controller - pos_control.update_xy_controller(); - pos_control.update_z_controller(); + position_control->update_xy_controller(); + position_control->update_z_controller(); float lateral_out, forward_out; - translate_pos_control_rp(lateral_out, forward_out); + sub.translate_pos_control_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); motors.set_forward(forward_out); // call attitude controller - if (auto_yaw_mode == AUTO_YAW_HOLD) { + if (sub.auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch & yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); } else { // roll, pitch from pilot, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); + attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); } } // guided_angle_control_run - runs the guided angle controller // called from guided_run -void Sub::guided_angle_control_run() +void ModeGuided::guided_angle_control_run() { // if motors not enabled set throttle to zero and exit immediately if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - attitude_control.set_throttle_out(0.0f,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); + attitude_control->set_throttle_out(0.0f,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); // initialise velocity controller - pos_control.init_z_controller(); + position_control->init_z_controller(); return; } @@ -465,7 +499,7 @@ void Sub::guided_angle_control_run() float roll_in = guided_angle_state.roll_cd; float pitch_in = guided_angle_state.pitch_cd; float total_in = norm(roll_in, pitch_in); - float angle_max = MIN(attitude_control.get_althold_lean_angle_max_cd(), aparm.angle_max); + float angle_max = MIN(attitude_control->get_althold_lean_angle_max_cd(), sub.aparm.angle_max); if (total_in > angle_max) { float ratio = angle_max / total_in; roll_in *= ratio; @@ -476,7 +510,7 @@ void Sub::guided_angle_control_run() float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd); // constrain climb rate - float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up()); + float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up()); // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds uint32_t tnow = AP_HAL::millis(); @@ -490,17 +524,17 @@ void Sub::guided_angle_control_run() motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // call attitude controller - attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true); + attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true); // call position controller - pos_control.set_pos_target_z_from_climb_rate_cm(climb_rate_cms); - pos_control.update_z_controller(); + position_control->set_pos_target_z_from_climb_rate_cm(climb_rate_cms); + position_control->update_z_controller(); } // Guided Limit code // guided_limit_clear - clear/turn off guided limits -void Sub::guided_limit_clear() +void ModeGuided::guided_limit_clear() { guided_limit.timeout_ms = 0; guided_limit.alt_min_cm = 0.0f; @@ -508,8 +542,97 @@ void Sub::guided_limit_clear() guided_limit.horiz_max_cm = 0.0f; } + +// set_auto_yaw_mode - sets the yaw mode for auto +void ModeGuided::set_auto_yaw_mode(autopilot_yaw_mode yaw_mode) +{ + // return immediately if no change + if (sub.auto_yaw_mode == yaw_mode) { + return; + } + sub.auto_yaw_mode = yaw_mode; + + // perform initialisation + switch (sub.auto_yaw_mode) { + + case AUTO_YAW_LOOK_AT_NEXT_WP: + // wpnav will initialise heading when wpnav's set_destination method is called + break; + + case AUTO_YAW_ROI: + // point towards a location held in yaw_look_at_WP + sub.yaw_look_at_WP_bearing = ahrs.yaw_sensor; + break; + + case AUTO_YAW_LOOK_AT_HEADING: + // keep heading pointing in the direction held in yaw_look_at_heading + // caller should set the yaw_look_at_heading + break; + + case AUTO_YAW_LOOK_AHEAD: + // Commanded Yaw to automatically look ahead. + sub.yaw_look_ahead_bearing = ahrs.yaw_sensor; + break; + + case AUTO_YAW_RESETTOARMEDYAW: + // initial_armed_bearing will be set during arming so no init required + break; + } +} + +// get_auto_heading - returns target heading depending upon auto_yaw_mode +// 100hz update rate +float ModeGuided::get_auto_heading() +{ + switch (sub.auto_yaw_mode) { + + case AUTO_YAW_ROI: + // point towards a location held in roi_WP + return sub.get_roi_yaw(); + break; + + case AUTO_YAW_LOOK_AT_HEADING: + // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed + return sub.yaw_look_at_heading; + break; + + case AUTO_YAW_LOOK_AHEAD: + // Commanded Yaw to automatically look ahead. + return sub.get_look_ahead_yaw(); + break; + + case AUTO_YAW_RESETTOARMEDYAW: + // changes yaw to be same as when quad was armed + return sub.initial_armed_bearing; + break; + + case AUTO_YAW_CORRECT_XTRACK: { + // TODO return current yaw if not in appropriate mode + // Bearing of current track (centidegrees) + float track_bearing = get_bearing_cd(sub.wp_nav.get_wp_origin().xy(), sub.wp_nav.get_wp_destination().xy()); + + // Bearing from current position towards intermediate position target (centidegrees) + const Vector2f target_vel_xy{position_control->get_vel_target_cms().x, position_control->get_vel_target_cms().y}; + float angle_error = 0.0f; + if (target_vel_xy.length() >= position_control->get_max_speed_xy_cms() * 0.1f) { + const float desired_angle_cd = degrees(target_vel_xy.angle()) * 100.0f; + angle_error = wrap_180_cd(desired_angle_cd - track_bearing); + } + float angle_limited = constrain_float(angle_error, -g.xtrack_angle_limit * 100.0f, g.xtrack_angle_limit * 100.0f); + return wrap_360_cd(track_bearing + angle_limited); + } + break; + + case AUTO_YAW_LOOK_AT_NEXT_WP: + default: + // point towards next waypoint. + // we don't use wp_bearing because we don't want the vehicle to turn too much during flight + return sub.wp_nav.get_yaw(); + break; + } +} // guided_limit_set - set guided timeout and movement limits -void Sub::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm) +void ModeGuided::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm) { guided_limit.timeout_ms = timeout_ms; guided_limit.alt_min_cm = alt_min_cm; @@ -519,7 +642,7 @@ void Sub::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_ // guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking // only called from AUTO mode's auto_nav_guided_start function -void Sub::guided_limit_init_time_and_pos() +void ModeGuided::guided_limit_init_time_and_pos() { // initialise start time guided_limit.start_time = AP_HAL::millis(); @@ -530,7 +653,7 @@ void Sub::guided_limit_init_time_and_pos() // guided_limit_check - returns true if guided mode has breached a limit // used when guided is invoked from the NAV_GUIDED_ENABLE mission command -bool Sub::guided_limit_check() +bool ModeGuided::guided_limit_check() { // check if we have passed the timeout if ((guided_limit.timeout_ms > 0) && (AP_HAL::millis() - guided_limit.start_time >= guided_limit.timeout_ms)) { diff --git a/ArduSub/control_manual.cpp b/ArduSub/control_manual.cpp index 93e821740a..9d6e29892e 100644 --- a/ArduSub/control_manual.cpp +++ b/ArduSub/control_manual.cpp @@ -1,36 +1,35 @@ #include "Sub.h" -// manual_init - initialise manual controller -bool Sub::manual_init() -{ + +bool ModeManual::init(bool ignore_checks) { // set target altitude to zero for reporting - pos_control.set_pos_target_z_cm(0); + position_control->set_pos_target_z_cm(0); // attitude hold inputs become thrust inputs in manual mode // set to neutral to prevent chaotic behavior (esp. roll/pitch) - set_neutral_controls(); + sub.set_neutral_controls(); return true; } // manual_run - runs the manual (passthrough) controller // should be called at 100hz or more -void Sub::manual_run() +void ModeManual::run() { // if not armed set throttle to zero and exit immediately - if (!motors.armed()) { - motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - attitude_control.set_throttle_out(0,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); + if (!sub.motors.armed()) { + sub.motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); return; } - motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + sub.motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - motors.set_roll(channel_roll->norm_input()); - motors.set_pitch(channel_pitch->norm_input()); - motors.set_yaw(channel_yaw->norm_input() * g.acro_yaw_p / ACRO_YAW_P); - motors.set_throttle(channel_throttle->norm_input()); - motors.set_forward(channel_forward->norm_input()); - motors.set_lateral(channel_lateral->norm_input()); + sub.motors.set_roll(channel_roll->norm_input()); + sub.motors.set_pitch(channel_pitch->norm_input()); + sub.motors.set_yaw(channel_yaw->norm_input() * g.acro_yaw_p / ACRO_YAW_P); + sub.motors.set_throttle(channel_throttle->norm_input()); + sub.motors.set_forward(channel_forward->norm_input()); + sub.motors.set_lateral(channel_lateral->norm_input()); } diff --git a/ArduSub/control_motordetect.cpp b/ArduSub/control_motordetect.cpp index 1718de53b4..370e1838ce 100644 --- a/ArduSub/control_motordetect.cpp +++ b/ArduSub/control_motordetect.cpp @@ -45,7 +45,7 @@ namespace { static int16_t current_direction; } -bool Sub::motordetect_init() +bool ModeMotordetect::init(bool ignore_checks) { current_motor = 0; md_state = STANDBY; @@ -53,7 +53,7 @@ bool Sub::motordetect_init() return true; } -void Sub::motordetect_run() +void ModeMotordetect::run() { // if not armed set throttle to zero and exit immediately if (!motors.armed()) { @@ -167,8 +167,8 @@ void Sub::motordetect_run() break; } case DONE: - control_mode = prev_control_mode; - arming.disarm(AP_Arming::Method::MOTORDETECTDONE); + set_mode(sub.prev_control_mode, ModeReason::MISSION_END); + sub.arming.disarm(AP_Arming::Method::MOTORDETECTDONE); break; } } diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp index 46121952ae..8c1b389464 100644 --- a/ArduSub/control_poshold.cpp +++ b/ArduSub/control_poshold.cpp @@ -7,47 +7,47 @@ #if POSHOLD_ENABLED == ENABLED // poshold_init - initialise PosHold controller -bool Sub::poshold_init() +bool ModePoshold::init(bool ignore_checks) { // fail to initialise PosHold mode if no GPS lock - if (!position_ok()) { + if (!sub.position_ok()) { return false; } // initialize vertical speeds and acceleration - pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); - pos_control.set_correction_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); - pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->set_max_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration()); + position_control->set_correction_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration()); + position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity - pos_control.init_xy_controller_stopping_point(); - pos_control.init_z_controller(); + position_control->init_xy_controller_stopping_point(); + position_control->init_z_controller(); // Stop all thrusters - attitude_control.set_throttle_out(0.5f ,true, g.throttle_filt); - attitude_control.relax_attitude_controllers(); - pos_control.relax_z_controller(0.5f); + attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt); + attitude_control->relax_attitude_controllers(); + position_control->relax_z_controller(0.5f); - last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading = ahrs.yaw_sensor; return true; } // poshold_run - runs the PosHold controller // should be called at 100hz or more -void Sub::poshold_run() +void ModePoshold::run() { uint32_t tnow = AP_HAL::millis(); // When unarmed, disable motors and stabilization if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) - attitude_control.set_throttle_out(0.5f ,true, g.throttle_filt); - attitude_control.relax_attitude_controllers(); - pos_control.init_xy_controller_stopping_point(); - pos_control.relax_z_controller(0.5f); - last_pilot_heading = ahrs.yaw_sensor; + attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt); + attitude_control->relax_attitude_controllers(); + position_control->init_xy_controller_stopping_point(); + position_control->relax_z_controller(0.5f); + sub.last_pilot_heading = ahrs.yaw_sensor; return; } @@ -62,15 +62,15 @@ void Sub::poshold_run() float lateral_out = 0; float forward_out = 0; - if (position_ok()) { + if (sub.position_ok()) { // Allow pilot to reposition the sub if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) { - pos_control.init_xy_controller_stopping_point(); + position_control->init_xy_controller_stopping_point(); } - translate_pos_control_rp(lateral_out, forward_out); - pos_control.update_xy_controller(); + sub.translate_pos_control_rp(lateral_out, forward_out); + position_control->update_xy_controller(); } else { - pos_control.init_xy_controller_stopping_point(); + position_control->init_xy_controller_stopping_point(); } motors.set_forward(forward_out + pilot_forward); motors.set_lateral(lateral_out + pilot_lateral); @@ -78,32 +78,32 @@ void Sub::poshold_run() // Update attitude // // get pilot's desired yaw rate - float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // convert pilot input to lean angles // To-Do: convert get_pilot_desired_lean_angles to return angles as floats float target_roll, target_pitch; - get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); + sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max); // update attitude controller targets if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - last_pilot_heading = ahrs.yaw_sensor; - last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading } else { // hold current heading // this check is required to prevent bounce back after very fast yaw maneuvers // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped - if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading + if (tnow < sub.last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading target_yaw_rate = 0; // Stop rotation on yaw axis // call attitude controller with target yaw rate = 0 to decelerate on yaw axis - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - last_pilot_heading = ahrs.yaw_sensor; // update heading to hold + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold } else { // call attitude controller holding absolute absolute bearing - attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true); + attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, sub.last_pilot_heading, true); } } diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp index 356ce2479b..cd75be241b 100644 --- a/ArduSub/control_stabilize.cpp +++ b/ArduSub/control_stabilize.cpp @@ -1,66 +1,65 @@ #include "Sub.h" -// stabilize_init - initialise stabilize controller -bool Sub::stabilize_init() -{ - // set target altitude to zero for reporting - pos_control.set_pos_target_z_cm(0); - last_pilot_heading = ahrs.yaw_sensor; +bool ModeStabilize::init(bool ignore_checks) { + // set target altitude to zero for reporting + position_control->set_pos_target_z_cm(0); + sub.last_pilot_heading = ahrs.yaw_sensor; + + return true; return true; } -// stabilize_run - runs the main stabilize controller -// should be called at 100hz or more -void Sub::stabilize_run() +void ModeStabilize::run() { - uint32_t tnow = AP_HAL::millis(); + uint32_t tnow = AP_HAL::millis(); float target_roll, target_pitch; // if not armed set throttle to zero and exit immediately if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - attitude_control.set_throttle_out(0,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); - last_pilot_heading = ahrs.yaw_sensor; + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); + sub.last_pilot_heading = ahrs.yaw_sensor; return; } motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // convert pilot input to lean angles - // To-Do: convert get_pilot_desired_lean_angles to return angles as floats - get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); + // To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats + // TODO2: move into mode.h + sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max); // get pilot's desired yaw rate - float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // call attitude controller // update attitude controller targets if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - last_pilot_heading = ahrs.yaw_sensor; - last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading } else { // hold current heading // this check is required to prevent bounce back after very fast yaw maneuvers // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped - if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading + if (tnow < sub.last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading target_yaw_rate = 0; // Stop rotation on yaw axis // call attitude controller with target yaw rate = 0 to decelerate on yaw axis - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - last_pilot_heading = ahrs.yaw_sensor; // update heading to hold + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold } else { // call attitude controller holding absolute absolute bearing - attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true); + attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, sub.last_pilot_heading, true); } } // output pilot's throttle - attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); + attitude_control->set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); //control_in is range -1000-1000 //radio_in is raw pwm value diff --git a/ArduSub/control_surface.cpp b/ArduSub/control_surface.cpp index 407b04ab5a..6bab07a067 100644 --- a/ArduSub/control_surface.cpp +++ b/ArduSub/control_surface.cpp @@ -1,24 +1,24 @@ #include "Sub.h" -bool Sub::surface_init() +bool ModeSurface::init(bool ignore_checks) { - if(!control_check_barometer()) { + if(!sub.control_check_barometer()) { return false; } // initialize vertical speeds and acceleration - pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity - pos_control.init_z_controller(); + position_control->init_z_controller(); return true; } -void Sub::surface_run() +void ModeSurface::run() { float target_roll, target_pitch; @@ -26,36 +26,36 @@ void Sub::surface_run() if (!motors.armed()) { motors.output_min(); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); - attitude_control.set_throttle_out(0,true,g.throttle_filt); - attitude_control.relax_attitude_controllers(); - pos_control.init_z_controller(); + attitude_control->set_throttle_out(0,true,g.throttle_filt); + attitude_control->relax_attitude_controllers(); + position_control->init_z_controller(); return; } // Already at surface, hold depth at surface - if (ap.at_surface) { - set_mode(ALT_HOLD, ModeReason::SURFACE_COMPLETE); + if (sub.ap.at_surface) { + set_mode(Mode::Number::ALT_HOLD, ModeReason::SURFACE_COMPLETE); } // convert pilot input to lean angles - // To-Do: convert get_pilot_desired_lean_angles to return angles as floats - get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); + // To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats + sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max); // get pilot's desired yaw rate - float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); // set target climb rate - float cmb_rate = constrain_float(fabsf(wp_nav.get_default_speed_up()), 1, pos_control.get_max_speed_up_cms()); + float cmb_rate = constrain_float(fabsf(sub.wp_nav.get_default_speed_up()), 1, position_control->get_max_speed_up_cms()); // record desired climb rate for logging - desired_climb_rate = cmb_rate; + sub.desired_climb_rate = cmb_rate; // update altitude target and call position controller - pos_control.set_pos_target_z_from_climb_rate_cm(cmb_rate); - pos_control.update_z_controller(); + position_control->set_pos_target_z_from_climb_rate_cm(cmb_rate); + position_control->update_z_controller(); // pilot has control for repositioning motors.set_forward(channel_forward->norm_input()); diff --git a/ArduSub/defines.h b/ArduSub/defines.h index b3c7109f10..20224305a9 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -29,20 +29,6 @@ enum autopilot_yaw_mode { AUTO_YAW_CORRECT_XTRACK = 6 // steer the sub in order to correct for crosstrack error during line following }; -// Auto Pilot Modes enumeration -enum control_mode_t : uint8_t { - STABILIZE = 0, // manual angle with manual depth/throttle - ACRO = 1, // manual body-frame angular rate with manual depth/throttle - ALT_HOLD = 2, // manual angle with automatic depth/throttle - AUTO = 3, // fully automatic waypoint control using mission commands - GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands - CIRCLE = 7, // automatic circular flight with automatic throttle - SURFACE = 9, // automatically return to surface, pilot maintains horizontal control - POSHOLD = 16, // automatic position hold with manual override, with automatic throttle - MANUAL = 19, // Pass-through input with no stabilization - MOTOR_DETECT = 20 // Automatically detect motors orientation -}; - // Acro Trainer types #define ACRO_TRAINER_DISABLED 0 #define ACRO_TRAINER_LEVELING 1 @@ -55,32 +41,7 @@ enum control_mode_t : uint8_t { #define WP_YAW_BEHAVIOR_LOOK_AHEAD 3 // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicotpers) #define WP_YAW_BEHAVIOR_CORRECT_XTRACK 4 // point towards intermediate position target during line following -// Auto modes -enum AutoMode { - Auto_WP, - Auto_CircleMoveToEdge, - Auto_Circle, - Auto_NavGuided, - Auto_Loiter, - Auto_TerrainRecover -}; -// Guided modes -enum GuidedMode { - Guided_WP, - Guided_Velocity, - Guided_PosVel, - Guided_Angle, -}; - -// RTL states -enum RTLState { - RTL_InitialClimb, - RTL_ReturnHome, - RTL_LoiterAtHome, - RTL_FinalDescent, - RTL_Land -}; // Logging parameters - only 32 messages are available to the vehicle here. enum LoggingParameters { diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp index 4b2fa212b7..5c0d08d324 100644 --- a/ArduSub/failsafe.cpp +++ b/ArduSub/failsafe.cpp @@ -88,9 +88,9 @@ void Sub::failsafe_sensors_check() gcs().send_text(MAV_SEVERITY_CRITICAL, "Depth sensor error!"); AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_SENSORS, LogErrorCode::BAD_DEPTH); - if (control_mode == ALT_HOLD || control_mode == SURFACE || mode_requires_GPS(control_mode)) { + if (control_mode == Mode::Number::ALT_HOLD || control_mode == Mode::Number::SURFACE || sub.flightmode->requires_GPS()) { // This should always succeed - if (!set_mode(MANUAL, ModeReason::BAD_DEPTH)) { + if (!set_mode(Mode::Number::MANUAL, ModeReason::BAD_DEPTH)) { // We should never get here arming.disarm(AP_Arming::Method::BADFLOWOFCONTROL); } @@ -156,7 +156,7 @@ void Sub::handle_battery_failsafe(const char* type_str, const int8_t action) switch((Failsafe_Action)action) { case Failsafe_Action_Surface: - set_mode(SURFACE, ModeReason::BATTERY_FAILSAFE); + set_mode(Mode::Number::SURFACE, ModeReason::BATTERY_FAILSAFE); break; case Failsafe_Action_Disarm: arming.disarm(AP_Arming::Method::BATTERYFAILSAFE); @@ -299,7 +299,7 @@ void Sub::failsafe_leak_check() // Handle failsafe action if (failsafe.leak && g.failsafe_leak == FS_LEAK_SURFACE && motors.armed()) { - set_mode(SURFACE, ModeReason::LEAK_FAILSAFE); + set_mode(Mode::Number::SURFACE, ModeReason::LEAK_FAILSAFE); } } @@ -352,11 +352,11 @@ void Sub::failsafe_gcs_check() if (g.failsafe_gcs == FS_GCS_DISARM) { arming.disarm(AP_Arming::Method::GCSFAILSAFE); } else if (g.failsafe_gcs == FS_GCS_HOLD && motors.armed()) { - if (!set_mode(ALT_HOLD, ModeReason::GCS_FAILSAFE)) { + if (!set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_FAILSAFE)) { arming.disarm(AP_Arming::Method::GCS_FAILSAFE_HOLDFAILED); } } else if (g.failsafe_gcs == FS_GCS_SURFACE && motors.armed()) { - if (!set_mode(SURFACE, ModeReason::GCS_FAILSAFE)) { + if (!set_mode(Mode::Number::SURFACE, ModeReason::GCS_FAILSAFE)) { arming.disarm(AP_Arming::Method::GCS_FAILSAFE_SURFACEFAILED); } } @@ -380,7 +380,7 @@ void Sub::failsafe_crash_check() } // return immediately if we are not in an angle stabilized flight mode - if (control_mode == ACRO || control_mode == MANUAL) { + if (control_mode == Mode::Number::ACRO || control_mode == Mode::Number::MANUAL) { last_crash_check_pass_ms = tnow; failsafe.crash = false; return; @@ -425,7 +425,7 @@ void Sub::failsafe_crash_check() void Sub::failsafe_terrain_check() { // trigger with 5 seconds of failures while in AUTO mode - bool valid_mode = (control_mode == AUTO || control_mode == GUIDED); + bool valid_mode = (control_mode == Mode::Number::AUTO || control_mode == Mode::Number::GUIDED); bool timeout = (failsafe.terrain_last_failure_ms - failsafe.terrain_first_failure_ms) > FS_TERRAIN_TIMEOUT_MS; bool trigger_event = valid_mode && timeout; @@ -470,7 +470,7 @@ void Sub::failsafe_terrain_on_event() AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED); // If rangefinder is enabled, we can recover from this failsafe - if (!rangefinder_state.enabled || !auto_terrain_recover_start()) { + if (!rangefinder_state.enabled || !sub.mode_auto.auto_terrain_recover_start()) { failsafe_terrain_act(); } @@ -482,14 +482,14 @@ void Sub::failsafe_terrain_act() { switch (g.failsafe_terrain) { case FS_TERRAIN_HOLD: - if (!set_mode(POSHOLD, ModeReason::TERRAIN_FAILSAFE)) { - set_mode(ALT_HOLD, ModeReason::TERRAIN_FAILSAFE); + if (!set_mode(Mode::Number::POSHOLD, ModeReason::TERRAIN_FAILSAFE)) { + set_mode(Mode::Number::ALT_HOLD, ModeReason::TERRAIN_FAILSAFE); } AP_Notify::events.failsafe_mode_change = 1; break; case FS_TERRAIN_SURFACE: - set_mode(SURFACE, ModeReason::TERRAIN_FAILSAFE); + set_mode(Mode::Number::SURFACE, ModeReason::TERRAIN_FAILSAFE); AP_Notify::events.failsafe_mode_change = 1; break; diff --git a/ArduSub/fence.cpp b/ArduSub/fence.cpp index 7d89c89cae..c39e84aebf 100644 --- a/ArduSub/fence.cpp +++ b/ArduSub/fence.cpp @@ -16,7 +16,7 @@ void Sub::fence_check() const uint8_t orig_breaches = fence.get_breaches(); // check for new breaches; new_breaches is bitmask of fence types breached - const uint8_t new_breaches = fence.check(); + const uint8_t new_breaches = sub.fence.check(); // if there is a new breach take action if (new_breaches) { diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp deleted file mode 100644 index 521586923f..0000000000 --- a/ArduSub/flight_mode.cpp +++ /dev/null @@ -1,230 +0,0 @@ -#include "Sub.h" - -// change flight mode and perform any necessary initialisation -// returns true if mode was successfully set -bool Sub::set_mode(control_mode_t mode, ModeReason reason) -{ - // boolean to record if flight mode could be set - bool success = false; - - // return immediately if we are already in the desired mode - if (mode == control_mode) { - prev_control_mode = control_mode; - - control_mode_reason = reason; - return true; - } - - switch (mode) { - case ACRO: - success = acro_init(); - break; - - case STABILIZE: - success = stabilize_init(); - break; - - case ALT_HOLD: - success = althold_init(); - break; - - case AUTO: - success = auto_init(); - break; - - case CIRCLE: - success = circle_init(); - break; - - case GUIDED: - success = guided_init(); - break; - - case SURFACE: - success = surface_init(); - break; - -#if POSHOLD_ENABLED == ENABLED - case POSHOLD: - success = poshold_init(); - break; -#endif - - case MANUAL: - success = manual_init(); - break; - - case MOTOR_DETECT: - success = motordetect_init(); - break; - - default: - success = false; - break; - } - - // update flight mode - if (success) { - // perform any cleanup required by previous flight mode - exit_mode(control_mode, mode); - - prev_control_mode = control_mode; - - control_mode = mode; - control_mode_reason = reason; - logger.Write_Mode(control_mode, control_mode_reason); - gcs().send_message(MSG_HEARTBEAT); - - // update notify object - notify_flight_mode(control_mode); - -#if AP_CAMERA_ENABLED - camera.set_is_auto_mode(control_mode == AUTO); -#endif - -#if AP_FENCE_ENABLED - // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover - // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe) - // but it should be harmless to disable the fence temporarily in these situations as well - fence.manual_recovery_start(); -#endif - } else { - // Log error that we failed to enter desired flight mode - AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); - } - - // return success or failure - return success; -} - -bool Sub::set_mode(const uint8_t new_mode, const ModeReason reason) -{ - static_assert(sizeof(control_mode_t) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number"); - return sub.set_mode((control_mode_t)new_mode, reason); -} - -// update_flight_mode - calls the appropriate attitude controllers based on flight mode -// called at 100hz or more -void Sub::update_flight_mode() -{ - switch (control_mode) { - case ACRO: - acro_run(); - break; - - case STABILIZE: - stabilize_run(); - break; - - case ALT_HOLD: - althold_run(); - break; - - case AUTO: - auto_run(); - break; - - case CIRCLE: - circle_run(); - break; - - case GUIDED: - guided_run(); - break; - - case SURFACE: - surface_run(); - break; - -#if POSHOLD_ENABLED == ENABLED - case POSHOLD: - poshold_run(); - break; -#endif - - case MANUAL: - manual_run(); - break; - - case MOTOR_DETECT: - motordetect_run(); - break; - - default: - break; - } -} - -// exit_mode - high level call to organise cleanup as a flight mode is exited -void Sub::exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode) -{ - // stop mission when we leave auto mode - if (old_control_mode == AUTO) { - if (mission.state() == AP_Mission::MISSION_RUNNING) { - mission.stop(); - } -#if HAL_MOUNT_ENABLED - camera_mount.set_mode_to_default(); -#endif // HAL_MOUNT_ENABLED - } -} - -// returns true or false whether mode requires GPS -bool Sub::mode_requires_GPS(control_mode_t mode) -{ - switch (mode) { - case AUTO: - case GUIDED: - case CIRCLE: - case POSHOLD: - return true; - default: - return false; - } - - return false; -} - -// mode_has_manual_throttle - returns true if the flight mode has a manual throttle (i.e. pilot directly controls throttle) -bool Sub::mode_has_manual_throttle(control_mode_t mode) -{ - switch (mode) { - case ACRO: - case STABILIZE: - case MANUAL: - return true; - default: - return false; - } - - return false; -} - -// mode_allows_arming - returns true if vehicle can be armed in the specified mode -// arming_from_gcs should be set to true if the arming request comes from the ground station -bool Sub::mode_allows_arming(control_mode_t mode, bool arming_from_gcs) -{ - return (mode_has_manual_throttle(mode) - || mode == ALT_HOLD - || mode == POSHOLD - || (arming_from_gcs&& mode == GUIDED) - ); -} - -// notify_flight_mode - sets notify object based on flight mode. Only used for OreoLED notify device -void Sub::notify_flight_mode(control_mode_t mode) -{ - switch (mode) { - case AUTO: - case GUIDED: - case CIRCLE: - case SURFACE: - // autopilot modes - AP_Notify::flags.autopilot_mode = true; - break; - default: - // all other are manual flight modes - AP_Notify::flags.autopilot_mode = false; - break; - } -} diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index baf5874053..5f0f83fe9d 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -1,4 +1,5 @@ #include "Sub.h" +#include "mode.h" // Functions that will handle joystick/gamepad input // ---------------------------------------------------------------------------- @@ -34,7 +35,7 @@ void Sub::init_joystick() lights1 = RC_Channels::rc_channel(8)->get_radio_min(); lights2 = RC_Channels::rc_channel(9)->get_radio_min(); - set_mode(MANUAL, ModeReason::RC_COMMAND); // Initialize flight mode + set_mode(Mode::Number::MANUAL, ModeReason::RC_COMMAND); // Initialize flight mode if (g.numGainSettings < 1) { g.numGainSettings.set_and_save(1); @@ -157,28 +158,28 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) break; case JSButton::button_function_t::k_mode_manual: - set_mode(MANUAL, ModeReason::RC_COMMAND); + set_mode(Mode::Number::MANUAL, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_stabilize: - set_mode(STABILIZE, ModeReason::RC_COMMAND); + set_mode(Mode::Number::STABILIZE, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_depth_hold: - set_mode(ALT_HOLD, ModeReason::RC_COMMAND); + set_mode(Mode::Number::ALT_HOLD, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_auto: - set_mode(AUTO, ModeReason::RC_COMMAND); + set_mode(Mode::Number::AUTO, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_guided: - set_mode(GUIDED, ModeReason::RC_COMMAND); + set_mode(Mode::Number::GUIDED, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_circle: - set_mode(CIRCLE, ModeReason::RC_COMMAND); + set_mode(Mode::Number::CIRCLE, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_acro: - set_mode(ACRO, ModeReason::RC_COMMAND); + set_mode(Mode::Number::ACRO, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mode_poshold: - set_mode(POSHOLD, ModeReason::RC_COMMAND); + set_mode(Mode::Number::POSHOLD, ModeReason::RC_COMMAND); break; case JSButton::button_function_t::k_mount_center: diff --git a/ArduSub/mode.cpp b/ArduSub/mode.cpp new file mode 100644 index 0000000000..323836c2f9 --- /dev/null +++ b/ArduSub/mode.cpp @@ -0,0 +1,292 @@ +#include "Sub.h" + +/* + constructor for Mode object + */ +Mode::Mode(void) : + g(sub.g), + g2(sub.g2), + inertial_nav(sub.inertial_nav), + ahrs(sub.ahrs), + motors(sub.motors), + channel_roll(sub.channel_roll), + channel_pitch(sub.channel_pitch), + channel_throttle(sub.channel_throttle), + channel_yaw(sub.channel_yaw), + channel_forward(sub.channel_forward), + channel_lateral(sub.channel_lateral), + position_control(&sub.pos_control), + attitude_control(&sub.attitude_control), + G_Dt(sub.G_Dt) +{ }; + +// return the static controller object corresponding to supplied mode +Mode *Sub::mode_from_mode_num(const Mode::Number mode) +{ + Mode *ret = nullptr; + + switch (mode) { + case Mode::Number::MANUAL: + ret = &mode_manual; + break; + case Mode::Number::STABILIZE: + ret = &mode_stabilize; + break; + case Mode::Number::ACRO: + ret = &mode_acro; + break; + case Mode::Number::ALT_HOLD: + ret = &mode_althold; + break; + case Mode::Number::POSHOLD: + ret = &mode_poshold; + break; + case Mode::Number::AUTO: + ret = &mode_auto; + break; + case Mode::Number::GUIDED: + ret = &mode_guided; + break; + case Mode::Number::CIRCLE: + ret = &mode_circle; + break; + case Mode::Number::SURFACE: + ret = &mode_surface; + break; + case Mode::Number::MOTOR_DETECT: + ret = &mode_motordetect; + break; + default: + break; + } + + return ret; +} + + +// set_mode - change flight mode and perform any necessary initialisation +// optional force parameter used to force the flight mode change (used only first time mode is set) +// returns true if mode was successfully set +// Some modes can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately +bool Sub::set_mode(Mode::Number mode, ModeReason reason) +{ + + // return immediately if we are already in the desired mode + if (mode == control_mode) { + control_mode_reason = reason; + return true; + } + + Mode *new_flightmode = mode_from_mode_num((Mode::Number)mode); + if (new_flightmode == nullptr) { + notify_no_such_mode((uint8_t)mode); + return false; + } + + if (new_flightmode->requires_GPS() && + !sub.position_ok()) { + gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s requires position", new_flightmode->name()); + AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + return false; + } + + // check for valid altitude if old mode did not require it but new one does + // we only want to stop changing modes if it could make things worse + if (!sub.control_check_barometer() && // maybe use ekf_alt_ok() instead? + flightmode->has_manual_throttle() && + !new_flightmode->has_manual_throttle()) { + gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s need alt estimate", new_flightmode->name()); + AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + return false; + } + + if (!new_flightmode->init(false)) { + gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed %s", new_flightmode->name()); + AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + return false; + } + + // perform any cleanup required by previous flight mode + exit_mode(flightmode, new_flightmode); + + // store previous flight mode (only used by tradeheli's autorotation) + prev_control_mode = control_mode; + + // update flight mode + flightmode = new_flightmode; + control_mode = mode; + control_mode_reason = reason; + logger.Write_Mode((uint8_t)control_mode, reason); + gcs().send_message(MSG_HEARTBEAT); + + // update notify object + notify_flight_mode(); + + // return success + return true; +} + +// exit_mode - high level call to organise cleanup as a flight mode is exited +void Sub::exit_mode(Mode::Number old_control_mode, Mode::Number new_control_mode) +{ + // stop mission when we leave auto mode + if (old_control_mode == Mode::Number::AUTO) { + if (mission.state() == AP_Mission::MISSION_RUNNING) { + mission.stop(); + } +#if HAL_MOUNT_ENABLED + camera_mount.set_mode_to_default(); +#endif // HAL_MOUNT_ENABLED + } +} + +bool Sub::set_mode(const uint8_t new_mode, const ModeReason reason) +{ + static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number"); + return sub.set_mode(static_cast(new_mode), reason); +} + +// update_flight_mode - calls the appropriate attitude controllers based on flight mode +// called at 100hz or more +void Sub::update_flight_mode() +{ + flightmode->run(); +} + +// exit_mode - high level call to organise cleanup as a flight mode is exited +void Sub::exit_mode(Mode *&old_flightmode, Mode *&new_flightmode){ +#if HAL_MOUNT_ENABLED + camera_mount.set_mode_to_default(); +#endif // HAL_MOUNT_ENABLED +} + +// notify_flight_mode - sets notify object based on current flight mode. Only used for OreoLED notify device +void Sub::notify_flight_mode() +{ + AP_Notify::flags.autopilot_mode = flightmode->is_autopilot(); + AP_Notify::flags.flight_mode = (uint8_t)control_mode; + notify.set_flight_mode_str(flightmode->name4()); +} + + +// get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates +// returns desired angle rates in centi-degrees-per-second +void Mode::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out) +{ + float rate_limit; + Vector3f rate_ef_level, rate_bf_level, rate_bf_request; + + // apply circular limit to pitch and roll inputs + float total_in = norm(pitch_in, roll_in); + + if (total_in > ROLL_PITCH_INPUT_MAX) { + float ratio = (float)ROLL_PITCH_INPUT_MAX / total_in; + roll_in *= ratio; + pitch_in *= ratio; + } + + // calculate roll, pitch rate requests + if (g.acro_expo <= 0) { + rate_bf_request.x = roll_in * g.acro_rp_p; + rate_bf_request.y = pitch_in * g.acro_rp_p; + } else { + // expo variables + float rp_in, rp_in3, rp_out; + + // range check expo + if (g.acro_expo > 1.0f) { + g.acro_expo.set(1.0f); + } + + // roll expo + rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX; + rp_in3 = rp_in*rp_in*rp_in; + rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in); + rate_bf_request.x = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p; + + // pitch expo + rp_in = float(pitch_in)/ROLL_PITCH_INPUT_MAX; + rp_in3 = rp_in*rp_in*rp_in; + rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in); + rate_bf_request.y = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p; + } + + // calculate yaw rate request + rate_bf_request.z = yaw_in * g.acro_yaw_p; + + // calculate earth frame rate corrections to pull the vehicle back to level while in ACRO mode + + if (g.acro_trainer != ACRO_TRAINER_DISABLED) { + // Calculate trainer mode earth frame rate command for roll + int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); + rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll; + + // Calculate trainer mode earth frame rate command for pitch + int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor); + rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch; + + // Calculate trainer mode earth frame rate command for yaw + rate_ef_level.z = 0; + + // Calculate angle limiting earth frame rate commands + if (g.acro_trainer == ACRO_TRAINER_LIMITED) { + if (roll_angle > sub.aparm.angle_max) { + rate_ef_level.x -= g.acro_balance_roll*(roll_angle-sub.aparm.angle_max); + } else if (roll_angle < -sub.aparm.angle_max) { + rate_ef_level.x -= g.acro_balance_roll*(roll_angle+sub.aparm.angle_max); + } + + if (pitch_angle > sub.aparm.angle_max) { + rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle-sub.aparm.angle_max); + } else if (pitch_angle < -sub.aparm.angle_max) { + rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle+sub.aparm.angle_max); + } + } + + // convert earth-frame level rates to body-frame level rates + attitude_control->euler_rate_to_ang_vel(attitude_control->get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level); + + // combine earth frame rate corrections with rate requests + if (g.acro_trainer == ACRO_TRAINER_LIMITED) { + rate_bf_request.x += rate_bf_level.x; + rate_bf_request.y += rate_bf_level.y; + rate_bf_request.z += rate_bf_level.z; + } else { + float acro_level_mix = constrain_float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch(); + + // Scale leveling rates by stick input + rate_bf_level = rate_bf_level*acro_level_mix; + + // Calculate rate limit to prevent change of rate through inverted + rate_limit = fabsf(fabsf(rate_bf_request.x)-fabsf(rate_bf_level.x)); + rate_bf_request.x += rate_bf_level.x; + rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit); + + // Calculate rate limit to prevent change of rate through inverted + rate_limit = fabsf(fabsf(rate_bf_request.y)-fabsf(rate_bf_level.y)); + rate_bf_request.y += rate_bf_level.y; + rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit); + + // Calculate rate limit to prevent change of rate through inverted + rate_limit = fabsf(fabsf(rate_bf_request.z)-fabsf(rate_bf_level.z)); + rate_bf_request.z += rate_bf_level.z; + rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit); + } + } + + // hand back rate request + roll_out = rate_bf_request.x; + pitch_out = rate_bf_request.y; + yaw_out = rate_bf_request.z; +} + + +bool Mode::set_mode(Mode::Number mode, ModeReason reason) +{ + return sub.set_mode(mode, reason); +} + +GCS_Sub &Mode::gcs() +{ + return sub.gcs(); +} diff --git a/ArduSub/mode.h b/ArduSub/mode.h new file mode 100644 index 0000000000..ffe01a525e --- /dev/null +++ b/ArduSub/mode.h @@ -0,0 +1,444 @@ +#pragma once + +#include "Sub.h" +class Parameters; +class ParametersG2; + +class GCS_Sub; + +// Guided modes +enum GuidedSubMode { + Guided_WP, + Guided_Velocity, + Guided_PosVel, + Guided_Angle, +}; + +// Auto modes +enum AutoSubMode { + Auto_WP, + Auto_CircleMoveToEdge, + Auto_Circle, + Auto_NavGuided, + Auto_Loiter, + Auto_TerrainRecover +}; + +// RTL states +enum RTLState { + RTL_InitialClimb, + RTL_ReturnHome, + RTL_LoiterAtHome, + RTL_FinalDescent, + RTL_Land +}; + +class Mode +{ + +public: + + // Auto Pilot Modes enumeration + enum class Number : uint8_t { + STABILIZE = 0, // manual angle with manual depth/throttle + ACRO = 1, // manual body-frame angular rate with manual depth/throttle + ALT_HOLD = 2, // manual angle with automatic depth/throttle + AUTO = 3, // fully automatic waypoint control using mission commands + GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands + CIRCLE = 7, // automatic circular flight with automatic throttle + SURFACE = 9, // automatically return to surface, pilot maintains horizontal control + POSHOLD = 16, // automatic position hold with manual override, with automatic throttle + MANUAL = 19, // Pass-through input with no stabilization + MOTOR_DETECT = 20 // Automatically detect motors orientation + }; + + // constructor + Mode(void); + + // do not allow copying + CLASS_NO_COPY(Mode); + + // child classes should override these methods + virtual bool init(bool ignore_checks) { return true; } + virtual void run() = 0; + virtual bool requires_GPS() const = 0; + virtual bool has_manual_throttle() const = 0; + virtual bool allows_arming(bool from_gcs) const = 0; + virtual bool is_autopilot() const { return false; } + virtual bool in_guided_mode() const { return false; } + + // return a string for this flightmode + virtual const char *name() const = 0; + virtual const char *name4() const = 0; + + // functions for reporting to GCS + virtual bool get_wp(Location &loc) { return false; } + virtual int32_t wp_bearing() const { return 0; } + virtual uint32_t wp_distance() const { return 0; } + virtual float crosstrack_error() const { return 0.0f; } + + + // pilot input processing + void get_pilot_desired_accelerations(float &right_out, float &front_out) const; + void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out); + + +protected: + + // navigation support functions + virtual void run_autopilot() {} + + // helper functions + bool is_disarmed_or_landed() const; + + // functions to control landing + // in modes that support landing + void land_run_horizontal_control(); + void land_run_vertical_control(bool pause_descent = false); + + // convenience references to avoid code churn in conversion: + Parameters &g; + ParametersG2 &g2; + AP_InertialNav &inertial_nav; + AP_AHRS &ahrs; + AP_Motors6DOF &motors; + RC_Channel *&channel_roll; + RC_Channel *&channel_pitch; + RC_Channel *&channel_throttle; + RC_Channel *&channel_yaw; + RC_Channel *&channel_forward; + RC_Channel *&channel_lateral; + AC_PosControl *position_control; + AC_AttitudeControl_Sub *attitude_control; + // TODO: channels + float &G_Dt; + +public: + // Navigation Yaw control + class AutoYaw + { + + public: + // mode(): current method of determining desired yaw: + autopilot_yaw_mode mode() const + { + return (autopilot_yaw_mode)_mode; + } + void set_mode_to_default(bool rtl); + void set_mode(autopilot_yaw_mode new_mode); + autopilot_yaw_mode default_mode(bool rtl) const; + + void set_rate(float new_rate_cds); + + // set_roi(...): set a "look at" location: + void set_roi(const Location &roi_location); + + void set_fixed_yaw(float angle_deg, + float turn_rate_dps, + int8_t direction, + bool relative_angle); + + private: + + // yaw_cd(): main product of AutoYaw; the heading: + float yaw_cd(); + + // rate_cds(): desired yaw rate in centidegrees/second: + float rate_cds(); + + float look_ahead_yaw(); + float roi_yaw(); + + // auto flight mode's yaw mode + uint8_t _mode = AUTO_YAW_LOOK_AT_NEXT_WP; + + // Yaw will point at this location if mode is set to AUTO_YAW_ROI + Vector3f roi; + + // bearing from current location to the ROI + float _roi_yaw; + + // yaw used for YAW_FIXED yaw_mode + int32_t _fixed_yaw; + + // Deg/s we should turn + int16_t _fixed_yaw_slewrate; + + // heading when in yaw_look_ahead_yaw + float _look_ahead_yaw; + + // used to reduce update rate to 100hz: + uint8_t roi_yaw_counter; + + GuidedSubMode guided_mode; + + }; + static AutoYaw auto_yaw; + + // pass-through functions to reduce code churn on conversion; + // these are candidates for moving into the Mode base + // class. + bool set_mode(Mode::Number mode, ModeReason reason); + GCS_Sub &gcs(); + + // end pass-through functions +}; + +class ModeManual : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + 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 is_autopilot() const override { return false; } + +protected: + + const char *name() const override { return "MANUAL"; } + const char *name4() const override { return "MANU"; } +}; + + +class ModeAcro : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + + 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 is_autopilot() const override { return false; } + +protected: + + const char *name() const override { return "ACRO"; } + const char *name4() const override { return "ACRO"; } +}; + + +class ModeStabilize : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + + 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 is_autopilot() const override { return false; } + +protected: + + const char *name() const override { return "STABILIZE"; } + const char *name4() const override { return "STAB"; } +}; + + +class ModeAlthold : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + + virtual void run() override; + + bool init(bool ignore_checks) override; + bool requires_GPS() const override { return false; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; } + bool is_autopilot() const override { return false; } + void control_depth(); + +protected: + + const char *name() const override { return "ALT_HOLD"; } + const char *name4() const override { return "ALTH"; } +}; + + +class ModeGuided : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + + virtual void run() override; + + bool init(bool ignore_checks) override; + bool requires_GPS() const override { return true; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; } + bool is_autopilot() const override { return true; } + bool guided_limit_check(); + void guided_limit_init_time_and_pos(); + void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads); + void guided_set_angle(const Quaternion&, float); + void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm); + bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity); + bool guided_set_destination(const Vector3f& destination); + bool guided_set_destination(const Location&); + void guided_set_velocity(const Vector3f& velocity); + float get_auto_heading(); + void guided_limit_clear(); + void set_auto_yaw_mode(autopilot_yaw_mode yaw_mode); + +protected: + + const char *name() const override { return "GUIDED"; } + const char *name4() const override { return "GUID"; } + autopilot_yaw_mode get_default_auto_yaw_mode(bool rtl) const; + +private: + void guided_pos_control_run(); + void guided_vel_control_run(); + void guided_posvel_control_run(); + void guided_angle_control_run(); + void guided_takeoff_run(); + void guided_pos_control_start(); + void guided_vel_control_start(); + void guided_posvel_control_start(); + void guided_angle_control_start(); +}; + + + +class ModeAuto : public ModeGuided +{ + +public: + // inherit constructor + using ModeGuided::ModeGuided; + + virtual void run() override; + + bool init(bool ignore_checks) override; + bool requires_GPS() const override { return true; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; } + bool is_autopilot() const override { return true; } + bool auto_loiter_start(); + void auto_wp_start(const Vector3f& destination); + void auto_wp_start(const Location& dest_loc); + void auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn); + void auto_circle_start(); + void auto_nav_guided_start(); + void set_auto_yaw_roi(const Location &roi_location); + void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle); + bool auto_terrain_recover_start(); + +protected: + + const char *name() const override { return "AUTO"; } + const char *name4() const override { return "AUTO"; } + +private: + void auto_wp_run(); + void auto_circle_run(); + void auto_nav_guided_run(); + void auto_loiter_run(); + void auto_terrain_recover_run(); +}; + + +class ModePoshold : public ModeAlthold +{ + +public: + // inherit constructor + using ModeAlthold::ModeAlthold; + + virtual void run() override; + + bool init(bool ignore_checks) override; + + bool requires_GPS() const override { return false; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; } + bool is_autopilot() const override { return true; } + +protected: + + const char *name() const override { return "POSHOLD"; } + const char *name4() const override { return "POSH"; } +}; + + +class ModeCircle : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + + virtual void run() override; + + bool init(bool ignore_checks) override; + bool requires_GPS() const override { return true; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; } + bool is_autopilot() const override { return true; } + +protected: + + const char *name() const override { return "CIRCLE"; } + const char *name4() const override { return "CIRC"; } +}; + +class ModeSurface : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + + virtual void run() override; + + bool init(bool ignore_checks) override; + bool requires_GPS() const override { return true; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; } + bool is_autopilot() const override { return true; } + +protected: + + const char *name() const override { return "SURFACE"; } + const char *name4() const override { return "SURF"; } +}; + + +class ModeMotordetect : public Mode +{ + +public: + // inherit constructor + using Mode::Mode; + + virtual void run() override; + + bool init(bool ignore_checks) override; + bool requires_GPS() const override { return true; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; } + bool is_autopilot() const override { return true; } + +protected: + + const char *name() const override { return "MOTORDETECT"; } + const char *name4() const override { return "DETE"; } +}; diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 5d33b5052b..9e25f51513 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -10,7 +10,7 @@ void Sub::enable_motor_output() void Sub::motors_output() { // Motor detection mode controls the thrusters directly - if (control_mode == MOTOR_DETECT){ + if (control_mode == Mode::Number::MOTOR_DETECT){ return; } // check if we are performing the motor test