Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: big mode refactor

This commit is contained in:
Willian Galvani 2023-04-03 22:11:21 +10:00
parent d31f0d2312
commit 9b70ac1286
26 changed files with 1365 additions and 1059 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/AP_Arming_Sub.cpp
View File

@ -134,7 +134,7 @@ bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks)
sub.motors.armed(true); sub.motors.armed(true);
// log flight mode in case it was changed while vehicle was disarmed // 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 // reenable failsafe
sub.mainloop_failsafe_enable(); sub.mainloop_failsafe_enable();

4
ArduSub/ArduSub.cpp
View File

@ -140,7 +140,7 @@ void Sub::run_rate_controller()
pos_control.set_dt(last_loop_time_s); pos_control.set_dt(last_loop_time_s);
//don't run rate controller in manual or motordetection modes //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 // run low level rate controllers that only require IMU data and set loop time
attitude_control.rate_controller_run(); attitude_control.rate_controller_run();
} }
@ -201,7 +201,7 @@ void Sub::ten_hz_logging_loop()
if (should_log(MASK_LOG_RCOUT)) { if (should_log(MASK_LOG_RCOUT)) {
logger.Write_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(); pos_control.write_log();
} }
if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) { if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {

52
ArduSub/GCS_Mavlink.cpp
View File

@ -21,10 +21,10 @@ MAV_MODE GCS_MAVLINK_Sub::base_mode() const
// the APM flight mode and has a well defined meaning in the // the APM flight mode and has a well defined meaning in the
// ArduPlane documentation // ArduPlane documentation
switch (sub.control_mode) { switch (sub.control_mode) {
case AUTO: case Mode::Number::AUTO:
case GUIDED: case Mode::Number::GUIDED:
case CIRCLE: case Mode::Number::CIRCLE:
case POSHOLD: case Mode::Number::POSHOLD:
_base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what // 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 // 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 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 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()) { if (!roi_loc.check_latlng()) {
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} }
sub.set_auto_yaw_roi(roi_loc); sub.mode_auto.set_auto_yaw_roi(roi_loc);
return MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
} }
@ -464,13 +464,13 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon
{ {
switch (packet.command) { switch (packet.command) {
case MAV_CMD_NAV_LOITER_UNLIM: 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_FAILED;
} }
return MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
case MAV_CMD_NAV_LAND: 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_FAILED;
} }
return MAV_RESULT_ACCEPTED; 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) && if ((packet.param1 >= 0.0f) &&
(packet.param1 <= 360.0f) && (packet.param1 <= 360.0f) &&
(is_zero(packet.param4) || is_equal(packet.param4,1.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_ACCEPTED;
} }
return MAV_RESULT_FAILED; 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; return MAV_RESULT_FAILED;
case MAV_CMD_MISSION_START: 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_ACCEPTED;
} }
return MAV_RESULT_FAILED; 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 // descend at up to WPNAV_SPEED_DN
climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * sub.wp_nav.get_default_speed_down(); 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; 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); mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
// exit if vehicle is not in Guided mode or Auto-Guided mode // 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; break;
} }
@ -652,11 +652,11 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg)
// send request // send request
if (!pos_ignore && !vel_ignore && acc_ignore) { 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) { } 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) { } else if (!pos_ignore && vel_ignore && acc_ignore) {
sub.guided_set_destination(pos_vector); sub.mode_guided.guided_set_destination(pos_vector);
} }
break; 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); mavlink_msg_set_position_target_global_int_decode(&msg, &packet);
// exit if vehicle is not in Guided, Auto-Guided, or Depth Hold modes // exit if vehicle is not in Guided, Auto-Guided, or Depth Hold modes
if ((sub.control_mode != GUIDED) if ((sub.control_mode != Mode::Number::GUIDED)
&& !(sub.control_mode == AUTO && sub.auto_mode == Auto_NavGuided) && !(sub.control_mode == Mode::Number::AUTO && sub.auto_mode == Auto_NavGuided)
&& !(sub.control_mode == ALT_HOLD)) { && !(sub.control_mode == Mode::Number::ALT_HOLD)) {
break; 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; * 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); sub.pos_control.set_pos_target_z_cm(packet.alt*100);
break; break;
} }
@ -715,11 +715,11 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg)
} }
if (!pos_ignore && !vel_ignore && acc_ignore) { 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) { } 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) { } 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; break;
@ -794,7 +794,7 @@ int16_t GCS_MAVLINK_Sub::high_latency_target_altitude() const
UNUSED_RESULT(ahrs.get_location(global_position_current)); UNUSED_RESULT(ahrs.get_location(global_position_current));
//return units are m //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.01 * (global_position_current.alt + sub.pos_control.get_pos_error_z_cm());
} }
return 0; 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 uint8_t GCS_MAVLINK_Sub::high_latency_tgt_heading() const
{ {
// return units are deg/2 // 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 // need to convert -18000->18000 to 0->360/2
return wrap_360_cd(sub.wp_nav.get_wp_bearing_to_destination()) / 200; 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 uint16_t GCS_MAVLINK_Sub::high_latency_tgt_dist() const
{ {
// return units are dm // 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 MIN(sub.wp_nav.get_wp_distance_to_destination() * 0.001, UINT16_MAX);
} }
return 0; 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 uint8_t GCS_MAVLINK_Sub::high_latency_tgt_airspeed() const
{ {
// return units are m/s*5 // 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 MIN((sub.pos_control.get_vel_desired_cms().length()/100) * 5, UINT8_MAX);
} }
return 0; return 0;

12
ArduSub/GCS_Sub.cpp
View File

@ -30,12 +30,12 @@ void GCS_Sub::update_vehicle_sensor_status_flags()
MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
switch (sub.control_mode) { switch (sub.control_mode) {
case ALT_HOLD: case Mode::Number::ALT_HOLD:
case AUTO: case Mode::Number::AUTO:
case GUIDED: case Mode::Number::GUIDED:
case CIRCLE: case Mode::Number::CIRCLE:
case SURFACE: case Mode::Number::SURFACE:
case POSHOLD: case Mode::Number::POSHOLD:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
control_sensors_health |= 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; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

2
ArduSub/Log.cpp
View File

@ -278,7 +278,7 @@ const struct LogStructure Sub::log_structure[] = {
void Sub::Log_Write_Vehicle_Startup_Messages() void Sub::Log_Write_Vehicle_Startup_Messages()
{ {
// only 200(?) bytes are guaranteed by AP_Logger // 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(); ahrs.Log_Write_Home_And_Origin();
gps.Write_AP_Logger_Log_Startup_messages(); gps.Write_AP_Logger_Log_Startup_messages();
} }

5
ArduSub/Sub.cpp
View File

@ -25,7 +25,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
*/ */
Sub::Sub() Sub::Sub()
: logger(g.log_bitmask), : logger(g.log_bitmask),
control_mode(MANUAL), control_mode(Mode::Number::MANUAL),
motors(MAIN_LOOP_RATE), motors(MAIN_LOOP_RATE),
auto_mode(Auto_WP), auto_mode(Auto_WP),
guided_mode(Guided_WP), guided_mode(Guided_WP),
@ -37,7 +37,8 @@ Sub::Sub()
wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control), wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control),
loiter_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), 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 #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
failsafe.pilot_input = true; failsafe.pilot_input = true;

108
ArduSub/Sub.h
View File

@ -71,6 +71,7 @@
#include "Parameters.h" #include "Parameters.h"
#include "AP_Arming_Sub.h" #include "AP_Arming_Sub.h"
#include "GCS_Sub.h" #include "GCS_Sub.h"
#include "mode.h"
#include <AP_OpticalFlow/AP_OpticalFlow.h> // Optical Flow library #include <AP_OpticalFlow/AP_OpticalFlow.h> // Optical Flow library
@ -108,6 +109,17 @@ public:
friend class ParametersG2; friend class ParametersG2;
friend class AP_Arming_Sub; friend class AP_Arming_Sub;
friend class RC_Channels_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); Sub(void);
@ -190,9 +202,9 @@ private:
// This is the state of the flight control system // This is the state of the flight control system
// There are multiple states defined such as STABILIZE, ACRO, // 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 #if RCMAP_ENABLED == ENABLED
RCMapper rcmap; RCMapper rcmap;
@ -234,12 +246,6 @@ private:
AP_Motors6DOF motors; 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 // Circle
bool circle_pilot_yaw_override; // true if pilot is overriding yaw bool circle_pilot_yaw_override; // true if pilot is overriding yaw
@ -416,61 +422,7 @@ private:
bool verify_wait_delay(); bool verify_wait_delay();
bool verify_within_distance(); bool verify_within_distance();
bool verify_yaw(); 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_sensors_check(void);
void failsafe_crash_check(); void failsafe_crash_check();
void failsafe_ekf_check(void); void failsafe_ekf_check(void);
@ -484,15 +436,12 @@ private:
void mainloop_failsafe_enable(); void mainloop_failsafe_enable();
void mainloop_failsafe_disable(); void mainloop_failsafe_disable();
void fence_check(); void fence_check();
bool set_mode(control_mode_t mode, ModeReason reason); bool set_mode(Mode::Number mode, ModeReason reason);
bool set_mode(const uint8_t mode, const ModeReason reason) override; bool set_mode(const uint8_t new_mode, const ModeReason reason) override;
uint8_t get_mode() const override { return (uint8_t)control_mode; } uint8_t get_mode() const override { return (uint8_t)control_mode; }
void update_flight_mode(); void update_flight_mode();
void exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode); void exit_mode(Mode::Number old_control_mode, Mode::Number new_control_mode);
bool mode_requires_GPS(control_mode_t mode); void notify_flight_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 read_inertia(); void read_inertia();
void update_surface_and_bottom_detector(); void update_surface_and_bottom_detector();
void set_surfaced(bool at_surface); void set_surfaced(bool at_surface);
@ -563,8 +512,7 @@ private:
void failsafe_internal_temperature_check(); void failsafe_internal_temperature_check();
void failsafe_terrain_act(void); 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_wpnav_rp(float &lateral_out, float &forward_out);
void translate_circle_nav_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, static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
"_failsafe_priorities is missing the sentinel"); "_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: public:
void mainloop_failsafe_check(); void mainloop_failsafe_check();

40
ArduSub/commands_logic.cpp
View File

@ -124,7 +124,7 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
// called by mission library in mission.update() // called by mission library in mission.update()
bool Sub::verify_command_callback(const AP_Mission::Mission_Command& cmd) 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); bool cmd_complete = verify_command(cmd);
// send message to GCS // send message to GCS
@ -207,8 +207,8 @@ void Sub::exit_mission()
AP_Notify::events.mission_complete = 1; AP_Notify::events.mission_complete = 1;
// Try to enter loiter, if that fails, go to depth hold // Try to enter loiter, if that fails, go to depth hold
if (!auto_loiter_start()) { if (!mode_auto.auto_loiter_start()) {
set_mode(ALT_HOLD, ModeReason::MISSION_END); 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; loiter_time_max = cmd.p1;
// Set wp navigation target // Set wp navigation target
auto_wp_start(target_loc); mode_auto.auto_wp_start(target_loc);
} }
// do_surface - initiate surface procedure // do_surface - initiate surface procedure
@ -279,12 +279,12 @@ void Sub::do_surface(const AP_Mission::Mission_Command& cmd)
} }
// Go to wp location // Go to wp location
auto_wp_start(target_location); mode_auto.auto_wp_start(target_location);
} }
void Sub::do_RTL() 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 // 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 // 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 // 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; 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 // 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 // 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) { if (cmd.p1 > 0) {
// initialise guided limits // initialise guided limits
guided_limit_init_time_and_pos(); mode_auto.guided_limit_init_time_and_pos();
// set navigation target // set navigation target
auto_nav_guided_start(); mode_auto.auto_nav_guided_start();
} }
} }
#endif // NAV_GUIDED #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 // do_guided_limits - pass guided limits to guided controller
void Sub::do_guided_limits(const AP_Mission::Mission_Command& cmd) 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_min * 100.0f, // convert meters to cm
cmd.content.guided_limits.alt_max * 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 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 // 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); 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 // advance to next state
auto_surface_state = AUTO_SURFACE_STATE_ASCEND; auto_surface_state = AUTO_SURFACE_STATE_ASCEND;
@ -529,13 +529,13 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)
} }
// start circling // start circling
auto_circle_start(); mode_auto.auto_circle_start();
} }
return false; return false;
} }
// check if we have completed circling // 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 #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 // check time and position limits
return guided_limit_check(); return mode_auto.guided_limit_check();
} }
#endif // NAV_GUIDED #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) 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.angle_deg,
cmd.content.yaw.turn_rate_dps, cmd.content.yaw.turn_rate_dps,
cmd.content.yaw.direction, 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) // 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) { 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 // 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) bool Sub::do_guided(const AP_Mission::Mission_Command& cmd)
{ {
// only process guided waypoint if we are in guided mode // 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; return false;
} }
@ -638,7 +638,7 @@ bool Sub::do_guided(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_NAV_WAYPOINT: { case MAV_CMD_NAV_WAYPOINT: {
// set wp_nav's destination // 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: 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 // 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) 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 // point the camera to a specified angle

138
ArduSub/control_acro.cpp
View File

@ -1,34 +1,30 @@
#include "Sub.h" #include "Sub.h"
/*
* control_acro.pde - init and run calls for acro flight mode
*/
// acro_init - initialise acro controller #include "Sub.h"
bool Sub::acro_init()
{
bool ModeAcro::init(bool ignore_checks) {
// set target altitude to zero for reporting // 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 // attitude hold inputs become thrust inputs in acro mode
// set to neutral to prevent chaotic behavior (esp. roll/pitch) // set to neutral to prevent chaotic behavior (esp. roll/pitch)
set_neutral_controls(); sub.set_neutral_controls();
return true; return true;
} }
// acro_run - runs the acro controller void ModeAcro::run()
// should be called at 100hz or more
void Sub::acro_run()
{ {
float target_roll, target_pitch, target_yaw; float target_roll, target_pitch, target_yaw;
// if not armed set throttle to zero and exit immediately // if not armed set throttle to zero and exit immediately
if (!motors.armed()) { if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control->relax_attitude_controllers();
return; 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); 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 // 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 // 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 //control_in is range 0-1000
//radio_in is raw pwm value //radio_in is raw pwm value
motors.set_forward(channel_forward->norm_input()); motors.set_forward(channel_forward->norm_input());
motors.set_lateral(channel_lateral->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;
}

70
ArduSub/control_althold.cpp
View File

@ -1,46 +1,40 @@
#include "Sub.h" #include "Sub.h"
/* bool ModeAlthold::init(bool ignore_checks) {
* control_althold.pde - init and run calls for althold, flight mode if(!sub.control_check_barometer()) {
*/
// althold_init - initialise althold controller
bool Sub::althold_init()
{
if(!control_check_barometer()) {
return false; return false;
} }
// initialize vertical maximum speeds and acceleration // initialize vertical maximum speeds and acceleration
// sets the maximum speed up and down returned by position controller // 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); position_control->set_max_speed_accel_z(-sub.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_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
// initialise position and desired velocity // 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; return true;
} }
// althold_run - runs the althold controller // althold_run - runs the althold controller
// should be called at 100hz or more // should be called at 100hz or more
void Sub::althold_run() void ModeAlthold::run()
{ {
uint32_t tnow = AP_HAL::millis(); uint32_t tnow = AP_HAL::millis();
// initialize vertical speeds and acceleration // 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()) { if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); 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) // 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->set_throttle_out(0.5,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control->relax_attitude_controllers();
pos_control.relax_z_controller(motors.get_throttle_hover()); position_control->relax_z_controller(motors.get_throttle_hover());
last_pilot_heading = ahrs.yaw_sensor; sub.last_pilot_heading = ahrs.yaw_sensor;
return; return;
} }
@ -53,7 +47,7 @@ void Sub::althold_run()
if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) { if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
float target_yaw; float target_yaw;
Quaternion( Quaternion(
set_attitude_target_no_gps.packet.q sub.set_attitude_target_no_gps.packet.q
).to_euler( ).to_euler(
target_roll, target_roll,
target_pitch, target_pitch,
@ -63,34 +57,34 @@ void Sub::althold_run()
target_pitch = degrees(target_pitch); target_pitch = degrees(target_pitch);
target_yaw = degrees(target_yaw); 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; 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 // 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 // call attitude controller
if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input 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); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
last_pilot_heading = ahrs.yaw_sensor; sub.last_pilot_heading = ahrs.yaw_sensor;
last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
} else { // hold current heading } else { // hold current heading
// this check is required to prevent bounce back after very fast yaw maneuvers // 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 // 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 target_yaw_rate = 0; // Stop rotation on yaw axis
// call attitude controller with target yaw rate = 0 to decelerate 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); 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 sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
} else { // call attitude controller holding absolute absolute bearing } 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()); motors.set_lateral(channel_lateral->norm_input());
} }
void Sub::control_depth() { void ModeAlthold::control_depth() {
float target_climb_rate_cm_s = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); 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, -get_pilot_speed_dn(), g.pilot_speed_up); 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. // 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 //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 (fabsf(target_climb_rate_cm_s) < 0.05f) {
if (ap.at_surface) { if (sub.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 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 (ap.at_bottom) { } else if (sub.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 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); position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s);
pos_control.update_z_controller(); position_control->update_z_controller();
} }

370
ArduSub/control_auto.cpp
View File

@ -7,42 +7,38 @@
* While in the auto flight mode, navigation or do/now commands can be run. * While in the auto flight mode, navigation or do/now commands can be run.
* Code in this file implements the navigation commands * Code in this file implements the navigation commands
*/ */
bool ModeAuto::init(bool ignore_checks) {
// auto_init - initialise auto controller if (!sub.position_ok() || sub.mission.num_commands() < 2) {
bool Sub::auto_init()
{
if (!position_ok() || mission.num_commands() < 2) {
return false; return false;
} }
auto_mode = Auto_Loiter; sub.auto_mode = Auto_Loiter;
// stop ROI from carrying over from previous runs of the mission // 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 // 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); set_auto_yaw_mode(AUTO_YAW_HOLD);
} }
// initialise waypoint controller // initialise waypoint controller
wp_nav.wp_and_spline_init(); sub.wp_nav.wp_and_spline_init();
// clear guided limits // clear guided limits
guided_limit_clear(); guided_limit_clear();
// start/resume the mission (based on MIS_RESTART parameter) // start/resume the mission (based on MIS_RESTART parameter)
mission.start_or_resume(); sub.mission.start_or_resume();
return true; return true;
} }
// auto_run - runs the appropriate auto controller // auto_run - runs the appropriate auto controller
// according to the current auto_mode // according to the current auto_mode
// should be called at 100hz or more void ModeAuto::run()
void Sub::auto_run()
{ {
mission.update(); sub.mission.update();
// call the correct auto controller // call the correct auto controller
switch (auto_mode) { switch (sub.auto_mode) {
case Auto_WP: case Auto_WP:
case Auto_CircleMoveToEdge: case Auto_CircleMoveToEdge:
@ -70,42 +66,42 @@ void Sub::auto_run()
} }
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination // 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) // 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 // 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 // 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)); set_auto_yaw_mode(get_default_auto_yaw_mode(false));
} }
} }
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination // 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 // 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 // failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event(); sub.failsafe_terrain_on_event();
return; return;
} }
// initialise yaw // 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 // 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)); set_auto_yaw_mode(get_default_auto_yaw_mode(false));
} }
} }
// auto_wp_run - runs the auto waypoint controller // auto_wp_run - runs the auto waypoint controller
// called by auto_run at 100hz or more // 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 not armed set throttle to zero and exit immediately
if (!motors.armed()) { if (!motors.armed()) {
@ -114,17 +110,17 @@ void Sub::auto_wp_run()
// call attitude controller // call attitude controller
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); 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; return;
} }
// process pilot's yaw input // process pilot's yaw input
float target_yaw_rate = 0; float target_yaw_rate = 0;
if (!failsafe.pilot_input) { if (!sub.failsafe.pilot_input) {
// get pilot's desired yaw rate // 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)) { if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD); 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 logic for terrain tracking target going below fence limit
// TODO implement waypoint radius individually for each waypoint based on cmd.p2 // 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 // 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 // // update xy outputs //
float lateral_out, forward_out; 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 // Send to forward/lateral outputs
motors.set_lateral(lateral_out); motors.set_lateral(lateral_out);
@ -151,52 +147,52 @@ void Sub::auto_wp_run()
// WP_Nav has set the vertical position control targets // WP_Nav has set the vertical position control targets
// run the vertical position controller and set output throttle // run the vertical position controller and set output throttle
pos_control.update_z_controller(); position_control->update_z_controller();
//////////////////////////// ////////////////////////////
// update attitude output // // update attitude output //
// get pilot desired lean angles // get pilot desired lean angles
float target_roll, target_pitch; 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 // call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) { if (sub.auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch & yaw rate from pilot // 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 { } else {
// roll, pitch from pilot, yaw heading from auto_heading() // 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 // 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 // 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 // set circle center
circle_nav.set_center(circle_center); sub.circle_nav.set_center(circle_center);
// set circle radius // set circle radius
if (!is_zero(radius_m)) { 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 // 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); 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 // check our distance from edge of circle
Vector3f circle_edge_neu; 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(); float dist_to_edge = (inertial_nav.get_position_neu_cm() - circle_edge_neu).length();
// if more than 3m then fly to edge // if more than 3m then fly to edge
if (dist_to_edge > 300.0f) { if (dist_to_edge > 300.0f) {
// set the state to move to the edge of the circle // 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 // convert circle_edge_neu to Location
Location circle_edge(circle_edge_neu, Location::AltFrame::ABOVE_ORIGIN); 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()); circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame());
// initialise wpnav to move to edge of circle // 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 // 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 // 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()); 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 > circle_nav.get_radius() && dist_to_center > 500) { if (dist_to_center > sub.circle_nav.get_radius() && dist_to_center > 500) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false)); set_auto_yaw_mode(get_default_auto_yaw_mode(false));
} else { } else {
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle // 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 // 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 // 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 // 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 // auto_circle_run - circle in AUTO flight mode
// called by auto_run at 100hz or more // called by auto_run at 100hz or more
void Sub::auto_circle_run() void ModeAuto::auto_circle_run()
{ {
// call circle controller // call circle controller
failsafe_terrain_set_status(circle_nav.update()); sub.failsafe_terrain_set_status(sub.circle_nav.update());
float lateral_out, forward_out; 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 // Send to forward/lateral outputs
motors.set_lateral(lateral_out); motors.set_lateral(lateral_out);
@ -249,50 +245,47 @@ void Sub::auto_circle_run()
// WP_Nav has set the vertical position control targets // WP_Nav has set the vertical position control targets
// run the vertical position controller and set output throttle // 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 // 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 #if NAV_GUIDED == ENABLED
// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode // 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; sub.mode_guided.init(true);
sub.auto_mode = Auto_NavGuided;
// call regular guided flight mode initialisation
guided_init(true);
// initialise guided start time and position as reference for limit checking // 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 // auto_nav_guided_run - allows control by external navigation controller
// called by auto_run at 100hz or more // 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 // call regular guided flight mode run function
guided_run(); sub.mode_guided.run();
} }
#endif // NAV_GUIDED #endif // NAV_GUIDED
// auto_loiter_start - initialises loitering in auto mode // auto_loiter_start - initialises loitering in auto mode
// returns success/failure because this can be called by exit_mission // 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 // return failure if GPS is bad
if (!position_ok()) { if (!sub.position_ok()) {
return false; return false;
} }
auto_mode = Auto_Loiter; sub.auto_mode = Auto_Loiter;
// calculate stopping point // calculate stopping point
Vector3f 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 // 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 // hold yaw at current heading
set_auto_yaw_mode(AUTO_YAW_HOLD); set_auto_yaw_mode(AUTO_YAW_HOLD);
@ -302,35 +295,35 @@ bool Sub::auto_loiter_start()
// auto_loiter_run - loiter in AUTO flight mode // auto_loiter_run - loiter in AUTO flight mode
// called by auto_run at 100hz or more // 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 not armed set throttle to zero and exit immediately
if (!motors.armed()) { if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); 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; return;
} }
// accept pilot input of yaw // accept pilot input of yaw
float target_yaw_rate = 0; float target_yaw_rate = 0;
if (!failsafe.pilot_input) { if (!sub.failsafe.pilot_input) {
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());
} }
// set motors to full range // set motors to full range
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run waypoint and z-axis position controller // 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 // // update xy outputs //
float lateral_out, forward_out; 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 // Send to forward/lateral outputs
motors.set_lateral(lateral_out); motors.set_lateral(lateral_out);
@ -338,102 +331,33 @@ void Sub::auto_loiter_run()
// WP_Nav has set the vertical position control targets // WP_Nav has set the vertical position control targets
// run the vertical position controller and set output throttle // run the vertical position controller and set output throttle
pos_control.update_z_controller(); position_control->update_z_controller();
// get pilot desired lean angles // get pilot desired lean angles
float target_roll, target_pitch; 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 // 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 // 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 // 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 // get final angle, 1 = Relative, 0 = Absolute
if (relative_angle == 0) { if (relative_angle == 0) {
// absolute angle // absolute angle
yaw_look_at_heading = wrap_360_cd(angle_deg * 100); sub.yaw_look_at_heading = wrap_360_cd(angle_deg * 100);
} else { } else {
// relative angle // relative angle
if (direction < 0) { if (direction < 0) {
angle_deg = -angle_deg; 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 // 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 // see AP_Float _slew_yaw in AC_AttitudeControl
if (is_zero(turn_rate_dps)) { if (is_zero(turn_rate_dps)) {
// default to regular auto slew rate // 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 { } else {
int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps; int32_t turn_rate = (wrap_180_cd(sub.yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps;
yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec sub.yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec
} }
// set yaw mode // 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 // 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 location is zero lat, lon and altitude turn off ROI
if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) { 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)); set_auto_yaw_mode(get_default_auto_yaw_mode(false));
#if HAL_MOUNT_ENABLED #if HAL_MOUNT_ENABLED
// switch off the camera tracking if enabled // switch off the camera tracking if enabled
if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { if (sub.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
camera_mount.set_mode_to_default(); sub.camera_mount.set_mode_to_default();
} }
#endif // HAL_MOUNT_ENABLED #endif // HAL_MOUNT_ENABLED
} else { } else {
#if HAL_MOUNT_ENABLED #if HAL_MOUNT_ENABLED
// check if mount type requires us to rotate the sub // check if mount type requires us to rotate the sub
if (!camera_mount.has_pan_control()) { if (!sub.camera_mount.has_pan_control()) {
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); set_auto_yaw_mode(AUTO_YAW_ROI);
} }
} }
// send the command to the camera mount // 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) // 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 // 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) // 4: point at a target given a target id (can't be implemented)
#else #else
// if we have no camera mount aim the sub at the location // 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); set_auto_yaw_mode(AUTO_YAW_ROI);
} }
#endif // HAL_MOUNT_ENABLED #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 // 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 // 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::OutOfRangeLow:
case RangeFinder::Status::OutOfRangeHigh: 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 // 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 // requires several consecutive valid readings for wpnav to accept rangefinder data
case RangeFinder::Status::Good: case RangeFinder::Status::Good:
auto_mode = Auto_TerrainRecover; sub.auto_mode = Auto_TerrainRecover;
break; break;
// Not connected or no data // Not connected or no data
@ -565,21 +437,21 @@ bool Sub::auto_terrain_recover_start()
} }
// Initialize recovery timeout time // Initialize recovery timeout time
fs_terrain_recover_start_ms = AP_HAL::millis(); sub.fs_terrain_recover_start_ms = AP_HAL::millis();
// Stop mission // Stop mission
mission.stop(); sub.mission.stop();
// Reset xy target // Reset xy target
loiter_nav.clear_pilot_desired_acceleration(); sub.loiter_nav.clear_pilot_desired_acceleration();
loiter_nav.init_target(); sub.loiter_nav.init_target();
// Reset z axis controller // 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 // 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()); 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());
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_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"); gcs().send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery");
return true; return true;
@ -588,7 +460,7 @@ bool Sub::auto_terrain_recover_start()
// Attempt recovery from terrain failsafe // Attempt recovery from terrain failsafe
// If recovery is successful resume mission // If recovery is successful resume mission
// If recovery fails revert to failsafe action // If recovery fails revert to failsafe action
void Sub::auto_terrain_recover_run() void ModeAuto::auto_terrain_recover_run()
{ {
float target_climb_rate = 0; float target_climb_rate = 0;
static uint32_t rangefinder_recovery_ms = 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 not armed set throttle to zero and exit immediately
if (!motors.armed()) { if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control->relax_attitude_controllers();
loiter_nav.init_target(); // Reset xy target sub.loiter_nav.init_target(); // Reset xy target
pos_control.relax_z_controller(motors.get_throttle_hover()); // Reset z axis controller position_control->relax_z_controller(motors.get_throttle_hover()); // Reset z axis controller
return; return;
} }
switch (rangefinder.status_orient(ROTATION_PITCH_270)) { switch (sub.rangefinder.status_orient(ROTATION_PITCH_270)) {
case RangeFinder::Status::OutOfRangeLow: 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; rangefinder_recovery_ms = 0;
break; break;
case RangeFinder::Status::OutOfRangeHigh: 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; rangefinder_recovery_ms = 0;
break; break;
@ -620,21 +492,21 @@ void Sub::auto_terrain_recover_run()
target_climb_rate = 0; // Attempt to hold current depth 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 // Start timer as soon as rangefinder is healthy
if (rangefinder_recovery_ms == 0) { if (rangefinder_recovery_ms == 0) {
rangefinder_recovery_ms = AP_HAL::millis(); 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 // 1.5 seconds of healthy rangefinder means we can resume mission with terrain enabled
if (AP_HAL::millis() > rangefinder_recovery_ms + 1500) { if (AP_HAL::millis() > rangefinder_recovery_ms + 1500) {
gcs().send_text(MAV_SEVERITY_INFO, "Terrain failsafe recovery successful!"); gcs().send_text(MAV_SEVERITY_INFO, "Terrain failsafe recovery successful!");
failsafe_terrain_set_status(true); // Reset failsafe timers sub.failsafe_terrain_set_status(true); // Reset failsafe timers
failsafe.terrain = false; // Clear flag sub.failsafe.terrain = false; // Clear flag
auto_mode = Auto_Loiter; // Switch back to loiter for next iteration sub.auto_mode = Auto_Loiter; // Switch back to loiter for next iteration
mission.resume(); // Resume mission sub.mission.resume(); // Resume mission
rangefinder_recovery_ms = 0; // Reset for subsequent recoveries 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 // Terrain failsafe recovery has failed, terrain data is not available
// and rangefinder is not connected, or has stopped responding // and rangefinder is not connected, or has stopped responding
gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery failure: No Rangefinder!"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery failure: No Rangefinder!");
failsafe_terrain_act(); sub.failsafe_terrain_act();
rangefinder_recovery_ms = 0; rangefinder_recovery_ms = 0;
return; return;
} }
// exit on failure (timeout) // 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 // Recovery has failed, revert to failsafe action
gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery timeout!"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery timeout!");
failsafe_terrain_act(); sub.failsafe_terrain_act();
} }
// run loiter controller // run loiter controller
loiter_nav.update(); sub.loiter_nav.update();
/////////////////////// ///////////////////////
// update xy targets // // update xy targets //
float lateral_out, forward_out; 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 // Send to forward/lateral outputs
motors.set_lateral(lateral_out); motors.set_lateral(lateral_out);
@ -672,8 +544,8 @@ void Sub::auto_terrain_recover_run()
///////////////////// /////////////////////
// update z target // // update z target //
pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate); position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
pos_control.update_z_controller(); position_control->update_z_controller();
//////////////////////////// ////////////////////////////
// update angular targets // // update angular targets //
@ -681,11 +553,11 @@ void Sub::auto_terrain_recover_run()
float target_pitch = 0; float target_pitch = 0;
// convert pilot input to lean angles // convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats // To-Do: convert sub.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); 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; float target_yaw_rate = 0;
// call attitude controller // 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);
} }

48
ArduSub/control_circle.cpp
View File

@ -5,82 +5,82 @@
*/ */
// circle_init - initialise circle controller flight mode // 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; return false;
} }
circle_pilot_yaw_override = false; sub.circle_pilot_yaw_override = false;
// initialize speeds and accelerations // initialize speeds and accelerations
pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); position_control->set_max_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration());
pos_control.set_correction_speed_accel_xy(wp_nav.get_default_speed_xy(), 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());
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);
pos_control.set_correction_speed_accel_z(-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 // initialise circle controller including setting the circle center based on vehicle speed
circle_nav.init(); sub.circle_nav.init();
return true; return true;
} }
// circle_run - runs the circle flight mode // circle_run - runs the circle flight mode
// should be called at 100hz or more // should be called at 100hz or more
void Sub::circle_run() void ModeCircle::run()
{ {
float target_yaw_rate = 0; float target_yaw_rate = 0;
float target_climb_rate = 0; float target_climb_rate = 0;
// update parameters, to allow changing at runtime // 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()); position_control->set_max_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.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_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 not armed set throttle to zero and exit immediately
if (!motors.armed()) { if (!motors.armed()) {
// To-Do: add some initialisation of position controllers // To-Do: add some initialisation of position controllers
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control->relax_attitude_controllers();
circle_nav.init(); sub.circle_nav.init();
return; return;
} }
// process pilot inputs // process pilot inputs
// get pilot's desired yaw rate // 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)) { if (!is_zero(target_yaw_rate)) {
circle_pilot_yaw_override = true; sub.circle_pilot_yaw_override = true;
} }
// get pilot desired climb rate // 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 // set motors to full range
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run circle controller // run circle controller
failsafe_terrain_set_status(circle_nav.update()); sub.failsafe_terrain_set_status(sub.circle_nav.update());
/////////////////////// ///////////////////////
// update xy outputs // // update xy outputs //
float lateral_out, forward_out; 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 // Send to forward/lateral outputs
motors.set_lateral(lateral_out); motors.set_lateral(lateral_out);
motors.set_forward(forward_out); motors.set_forward(forward_out);
// call attitude controller // call attitude controller
if (circle_pilot_yaw_override) { 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); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
} else { } 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 // update altitude target and call position controller
pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate); position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
pos_control.update_z_controller(); position_control->update_z_controller();
} }

331
ArduSub/control_guided.cpp
View File

@ -29,9 +29,9 @@ struct Guided_Limit {
} guided_limit; } guided_limit;
// guided_init - initialise guided controller // 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; return false;
} }
@ -40,73 +40,107 @@ bool Sub::guided_init(bool ignore_checks)
return true; 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 // initialise guided mode's position controller
void Sub::guided_pos_control_start() void ModeGuided::guided_pos_control_start()
{ {
// set to position control mode // set to position control mode
guided_mode = Guided_WP; sub.guided_mode = Guided_WP;
// initialise waypoint controller // initialise waypoint controller
wp_nav.wp_and_spline_init(); sub.wp_nav.wp_and_spline_init();
// initialise wpnav to stopping point at current altitude // initialise wpnav to stopping point at current altitude
// To-Do: set to current location if disarmed? // To-Do: set to current location if disarmed?
// To-Do: set to stopping point altitude? // To-Do: set to stopping point altitude?
Vector3f stopping_point; 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 // 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 // initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false)); set_auto_yaw_mode(get_default_auto_yaw_mode(false));
} }
// initialise guided mode's velocity controller // initialise guided mode's velocity controller
void Sub::guided_vel_control_start() void ModeGuided::guided_vel_control_start()
{ {
// set guided_mode to velocity controller // set guided_mode to velocity controller
guided_mode = Guided_Velocity; sub.guided_mode = Guided_Velocity;
// initialize vertical maximum speeds and acceleration // 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); position_control->set_max_speed_accel_z(-sub.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_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
// initialise velocity controller // initialise velocity controller
pos_control.init_z_controller(); position_control->init_z_controller();
pos_control.init_xy_controller(); position_control->init_xy_controller();
} }
// initialise guided mode's posvel 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 // set guided_mode to velocity controller
guided_mode = Guided_PosVel; sub.guided_mode = Guided_PosVel;
// set vertical speed and acceleration // 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()); 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());
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_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 // initialise velocity controller
pos_control.init_z_controller(); position_control->init_z_controller();
pos_control.init_xy_controller(); position_control->init_xy_controller();
// pilot always controls yaw // pilot always controls yaw
set_auto_yaw_mode(AUTO_YAW_HOLD); set_auto_yaw_mode(AUTO_YAW_HOLD);
} }
// initialise guided mode's angle controller // initialise guided mode's angle controller
void Sub::guided_angle_control_start() void ModeGuided::guided_angle_control_start()
{ {
// set guided_mode to velocity controller // set guided_mode to velocity controller
guided_mode = Guided_Angle; sub.guided_mode = Guided_Angle;
// set vertical speed and acceleration // 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()); 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());
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_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 // initialise velocity controller
pos_control.init_z_controller(); position_control->init_z_controller();
// initialise targets // initialise targets
guided_angle_state.update_time_ms = AP_HAL::millis(); 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 // guided_set_destination - sets guided mode's target destination
// Returns true if the fence is enabled and guided waypoint is within the fence // Returns true if the fence is enabled and guided waypoint is within the fence
// else return false if the waypoint is outside 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 // ensure we are in position control mode
if (guided_mode != Guided_WP) { if (sub.guided_mode != Guided_WP) {
guided_pos_control_start(); guided_pos_control_start();
} }
#if AP_FENCE_ENABLED #if AP_FENCE_ENABLED
// reject destination if outside the fence // reject destination if outside the fence
const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); 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); AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK // failure is propagated to GCS with NAK
return false; return false;
@ -140,34 +174,34 @@ bool Sub::guided_set_destination(const Vector3f& destination)
#endif #endif
// no need to check return status because terrain data is not used // 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 target
Log_Write_GuidedTarget(guided_mode, destination, Vector3f()); sub.Log_Write_GuidedTarget(sub.guided_mode, destination, Vector3f());
return true; return true;
} }
// sets guided mode's target from a Location object // sets guided mode's target from a Location object
// returns false if destination could not be set (probably caused by missing terrain data) // 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 // 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 // ensure we are in position control mode
if (guided_mode != Guided_WP) { if (sub.guided_mode != Guided_WP) {
guided_pos_control_start(); guided_pos_control_start();
} }
#if AP_FENCE_ENABLED #if AP_FENCE_ENABLED
// reject destination outside the fence. // 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 // 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); AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK // failure is propagated to GCS with NAK
return false; return false;
} }
#endif #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 // failure to set destination can only be because of missing terrain data
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
// failure is propagated to GCS with NAK // failure is propagated to GCS with NAK
@ -175,36 +209,36 @@ bool Sub::guided_set_destination(const Location& dest_loc)
} }
// log target // 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; return true;
} }
// guided_set_velocity - sets guided mode's target velocity // 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 // check we are in velocity control mode
if (guided_mode != Guided_Velocity) { if (sub.guided_mode != Guided_Velocity) {
guided_vel_control_start(); guided_vel_control_start();
} }
update_time_ms = AP_HAL::millis(); update_time_ms = AP_HAL::millis();
// set position controller velocity target // set position controller velocity target
pos_control.set_vel_desired_cms(velocity); position_control->set_vel_desired_cms(velocity);
} }
// set guided mode posvel target // 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 // check we are in velocity control mode
if (guided_mode != Guided_PosVel) { if (sub.guided_mode != Guided_PosVel) {
guided_posvel_control_start(); guided_posvel_control_start();
} }
#if AP_FENCE_ENABLED #if AP_FENCE_ENABLED
// reject destination if outside the fence // reject destination if outside the fence
const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); 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); AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK // failure is propagated to GCS with NAK
return false; return false;
@ -215,21 +249,21 @@ bool Sub::guided_set_destination_posvel(const Vector3f& destination, const Vecto
posvel_pos_target_cm = destination.topostype(); posvel_pos_target_cm = destination.topostype();
posvel_vel_target_cms = velocity; 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; 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; posvel_pos_target_cm.z = dz;
// log target // log target
Log_Write_GuidedTarget(guided_mode, destination, velocity); sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity);
return true; return true;
} }
// set guided mode angle target // 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 // check we are in velocity control mode
if (guided_mode != Guided_Angle) { if (sub.guided_mode != Guided_Angle) {
guided_angle_control_start(); 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 // guided_run - runs the guided controller
// should be called at 100hz or more // should be called at 100hz or more
void Sub::guided_run() void ModeGuided::run()
{ {
// call the correct auto controller // call the correct auto controller
switch (guided_mode) { switch (sub.guided_mode) {
case Guided_WP: case Guided_WP:
// run position controller // run position controller
@ -274,23 +308,23 @@ void Sub::guided_run()
// guided_pos_control_run - runs the guided position controller // guided_pos_control_run - runs the guided position controller
// called from guided_run // 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 not enabled set throttle to zero and exit immediately
if (!motors.armed()) { if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control->relax_attitude_controllers();
wp_nav.wp_and_spline_init(); sub.wp_nav.wp_and_spline_init();
return; return;
} }
// process pilot's yaw input // process pilot's yaw input
float target_yaw_rate = 0; float target_yaw_rate = 0;
if (!failsafe.pilot_input) { if (!sub.failsafe.pilot_input) {
// get pilot's desired yaw rate // 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)) { if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD); 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); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run waypoint controller // 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; 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 // Send to forward/lateral outputs
motors.set_lateral(lateral_out); motors.set_lateral(lateral_out);
@ -311,39 +345,39 @@ void Sub::guided_pos_control_run()
// WP_Nav has set the vertical position control targets // WP_Nav has set the vertical position control targets
// run the vertical position controller and set output throttle // run the vertical position controller and set output throttle
pos_control.update_z_controller(); position_control->update_z_controller();
// call attitude 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 // 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 { } else {
// roll, pitch from pilot, yaw heading from auto_heading() // 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 // guided_vel_control_run - runs the guided velocity controller
// called from guided_run // 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 // ifmotors not enabled set throttle to zero and exit immediately
if (!motors.armed()) { if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control->relax_attitude_controllers();
// initialise velocity controller // initialise velocity controller
pos_control.init_z_controller(); position_control->init_z_controller();
pos_control.init_xy_controller(); position_control->init_xy_controller();
return; return;
} }
// process pilot's yaw input // process pilot's yaw input
float target_yaw_rate = 0; float target_yaw_rate = 0;
if (!failsafe.pilot_input) { if (!sub.failsafe.pilot_input) {
// get pilot's desired yaw rate // 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)) { if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD); 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 // set velocity to zero if no updates received for 3 seconds
uint32_t tnow = AP_HAL::millis(); uint32_t tnow = AP_HAL::millis();
if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_vel_desired_cms().is_zero()) { if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !position_control->get_vel_desired_cms().is_zero()) {
pos_control.set_vel_desired_cms(Vector3f(0,0,0)); 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 // call velocity controller which includes z axis controller
pos_control.update_xy_controller(); position_control->update_xy_controller();
pos_control.update_z_controller(); position_control->update_z_controller();
float lateral_out, forward_out; 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 // Send to forward/lateral outputs
motors.set_lateral(lateral_out); motors.set_lateral(lateral_out);
motors.set_forward(forward_out); motors.set_forward(forward_out);
// call attitude 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 // 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 { } else {
// roll, pitch from pilot, yaw heading from auto_heading() // 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 // guided_posvel_control_run - runs the guided posvel controller
// called from guided_run // 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 not enabled set throttle to zero and exit immediately
if (!motors.armed()) { if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control->relax_attitude_controllers();
// initialise velocity controller // initialise velocity controller
pos_control.init_z_controller(); position_control->init_z_controller();
pos_control.init_xy_controller(); position_control->init_xy_controller();
return; return;
} }
// process pilot's yaw input // process pilot's yaw input
float target_yaw_rate = 0; float target_yaw_rate = 0;
if (!failsafe.pilot_input) { if (!sub.failsafe.pilot_input) {
// get pilot's desired yaw rate // 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)) { if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD); set_auto_yaw_mode(AUTO_YAW_HOLD);
} }
@ -417,47 +451,47 @@ void Sub::guided_posvel_control_run()
} }
// advance position target using velocity target // 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 // 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; 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; posvel_pos_target_cm.z = pz;
// run position controller // run position controller
pos_control.update_xy_controller(); position_control->update_xy_controller();
pos_control.update_z_controller(); position_control->update_z_controller();
float lateral_out, forward_out; 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 // Send to forward/lateral outputs
motors.set_lateral(lateral_out); motors.set_lateral(lateral_out);
motors.set_forward(forward_out); motors.set_forward(forward_out);
// call attitude 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 // 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 { } else {
// roll, pitch from pilot, yaw heading from auto_heading() // 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 // guided_angle_control_run - runs the guided angle controller
// called from guided_run // 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 not enabled set throttle to zero and exit immediately
if (!motors.armed()) { if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0.0f,true,g.throttle_filt); attitude_control->set_throttle_out(0.0f,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control->relax_attitude_controllers();
// initialise velocity controller // initialise velocity controller
pos_control.init_z_controller(); position_control->init_z_controller();
return; return;
} }
@ -465,7 +499,7 @@ void Sub::guided_angle_control_run()
float roll_in = guided_angle_state.roll_cd; float roll_in = guided_angle_state.roll_cd;
float pitch_in = guided_angle_state.pitch_cd; float pitch_in = guided_angle_state.pitch_cd;
float total_in = norm(roll_in, pitch_in); 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) { if (total_in > angle_max) {
float ratio = angle_max / total_in; float ratio = angle_max / total_in;
roll_in *= ratio; roll_in *= ratio;
@ -476,7 +510,7 @@ void Sub::guided_angle_control_run()
float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd); float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);
// constrain climb rate // 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 // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
uint32_t tnow = AP_HAL::millis(); 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); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// call attitude controller // 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 // call position controller
pos_control.set_pos_target_z_from_climb_rate_cm(climb_rate_cms); position_control->set_pos_target_z_from_climb_rate_cm(climb_rate_cms);
pos_control.update_z_controller(); position_control->update_z_controller();
} }
// Guided Limit code // Guided Limit code
// guided_limit_clear - clear/turn off guided limits // 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.timeout_ms = 0;
guided_limit.alt_min_cm = 0.0f; guided_limit.alt_min_cm = 0.0f;
@ -508,8 +542,97 @@ void Sub::guided_limit_clear()
guided_limit.horiz_max_cm = 0.0f; 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 // 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.timeout_ms = timeout_ms;
guided_limit.alt_min_cm = alt_min_cm; 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 // 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 // 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 // initialise start time
guided_limit.start_time = AP_HAL::millis(); 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 // guided_limit_check - returns true if guided mode has breached a limit
// used when guided is invoked from the NAV_GUIDED_ENABLE mission command // 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 // check if we have passed the timeout
if ((guided_limit.timeout_ms > 0) && (AP_HAL::millis() - guided_limit.start_time >= guided_limit.timeout_ms)) { if ((guided_limit.timeout_ms > 0) && (AP_HAL::millis() - guided_limit.start_time >= guided_limit.timeout_ms)) {

33
ArduSub/control_manual.cpp
View File

@ -1,36 +1,35 @@
#include "Sub.h" #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 // 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 // attitude hold inputs become thrust inputs in manual mode
// set to neutral to prevent chaotic behavior (esp. roll/pitch) // set to neutral to prevent chaotic behavior (esp. roll/pitch)
set_neutral_controls(); sub.set_neutral_controls();
return true; return true;
} }
// manual_run - runs the manual (passthrough) controller // manual_run - runs the manual (passthrough) controller
// should be called at 100hz or more // 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 not armed set throttle to zero and exit immediately
if (!motors.armed()) { if (!sub.motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); sub.motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control->relax_attitude_controllers();
return; 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()); sub.motors.set_roll(channel_roll->norm_input());
motors.set_pitch(channel_pitch->norm_input()); sub.motors.set_pitch(channel_pitch->norm_input());
motors.set_yaw(channel_yaw->norm_input() * g.acro_yaw_p / ACRO_YAW_P); sub.motors.set_yaw(channel_yaw->norm_input() * g.acro_yaw_p / ACRO_YAW_P);
motors.set_throttle(channel_throttle->norm_input()); sub.motors.set_throttle(channel_throttle->norm_input());
motors.set_forward(channel_forward->norm_input()); sub.motors.set_forward(channel_forward->norm_input());
motors.set_lateral(channel_lateral->norm_input()); sub.motors.set_lateral(channel_lateral->norm_input());
} }

8
ArduSub/control_motordetect.cpp
View File

@ -45,7 +45,7 @@ namespace {
static int16_t current_direction; static int16_t current_direction;
} }
bool Sub::motordetect_init() bool ModeMotordetect::init(bool ignore_checks)
{ {
current_motor = 0; current_motor = 0;
md_state = STANDBY; md_state = STANDBY;
@ -53,7 +53,7 @@ bool Sub::motordetect_init()
return true; return true;
} }
void Sub::motordetect_run() void ModeMotordetect::run()
{ {
// if not armed set throttle to zero and exit immediately // if not armed set throttle to zero and exit immediately
if (!motors.armed()) { if (!motors.armed()) {
@ -167,8 +167,8 @@ void Sub::motordetect_run()
break; break;
} }
case DONE: case DONE:
control_mode = prev_control_mode; set_mode(sub.prev_control_mode, ModeReason::MISSION_END);
arming.disarm(AP_Arming::Method::MOTORDETECTDONE); sub.arming.disarm(AP_Arming::Method::MOTORDETECTDONE);
break; break;
} }
} }

64
ArduSub/control_poshold.cpp
View File

@ -7,47 +7,47 @@
#if POSHOLD_ENABLED == ENABLED #if POSHOLD_ENABLED == ENABLED
// poshold_init - initialise PosHold controller // poshold_init - initialise PosHold controller
bool Sub::poshold_init() bool ModePoshold::init(bool ignore_checks)
{ {
// fail to initialise PosHold mode if no GPS lock // fail to initialise PosHold mode if no GPS lock
if (!position_ok()) { if (!sub.position_ok()) {
return false; return false;
} }
// initialize vertical speeds and acceleration // initialize vertical speeds and acceleration
pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); position_control->set_max_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration());
pos_control.set_correction_speed_accel_xy(wp_nav.get_default_speed_xy(), 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());
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);
pos_control.set_correction_speed_accel_z(-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 // initialise position and desired velocity
pos_control.init_xy_controller_stopping_point(); position_control->init_xy_controller_stopping_point();
pos_control.init_z_controller(); position_control->init_z_controller();
// Stop all thrusters // Stop all thrusters
attitude_control.set_throttle_out(0.5f ,true, g.throttle_filt); attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control->relax_attitude_controllers();
pos_control.relax_z_controller(0.5f); position_control->relax_z_controller(0.5f);
last_pilot_heading = ahrs.yaw_sensor; sub.last_pilot_heading = ahrs.yaw_sensor;
return true; return true;
} }
// poshold_run - runs the PosHold controller // poshold_run - runs the PosHold controller
// should be called at 100hz or more // should be called at 100hz or more
void Sub::poshold_run() void ModePoshold::run()
{ {
uint32_t tnow = AP_HAL::millis(); uint32_t tnow = AP_HAL::millis();
// When unarmed, disable motors and stabilization // When unarmed, disable motors and stabilization
if (!motors.armed()) { if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); 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) // 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->set_throttle_out(0.5f ,true, g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control->relax_attitude_controllers();
pos_control.init_xy_controller_stopping_point(); position_control->init_xy_controller_stopping_point();
pos_control.relax_z_controller(0.5f); position_control->relax_z_controller(0.5f);
last_pilot_heading = ahrs.yaw_sensor; sub.last_pilot_heading = ahrs.yaw_sensor;
return; return;
} }
@ -62,15 +62,15 @@ void Sub::poshold_run()
float lateral_out = 0; float lateral_out = 0;
float forward_out = 0; float forward_out = 0;
if (position_ok()) { if (sub.position_ok()) {
// Allow pilot to reposition the sub // Allow pilot to reposition the sub
if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) { 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); sub.translate_pos_control_rp(lateral_out, forward_out);
pos_control.update_xy_controller(); position_control->update_xy_controller();
} else { } else {
pos_control.init_xy_controller_stopping_point(); position_control->init_xy_controller_stopping_point();
} }
motors.set_forward(forward_out + pilot_forward); motors.set_forward(forward_out + pilot_forward);
motors.set_lateral(lateral_out + pilot_lateral); motors.set_lateral(lateral_out + pilot_lateral);
@ -78,32 +78,32 @@ void Sub::poshold_run()
// Update attitude // // Update attitude //
// get pilot's desired yaw rate // 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 // convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats // To-Do: convert get_pilot_desired_lean_angles to return angles as floats
float target_roll, target_pitch; 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 // update attitude controller targets
if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input 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); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
last_pilot_heading = ahrs.yaw_sensor; sub.last_pilot_heading = ahrs.yaw_sensor;
last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
} else { // hold current heading } else { // hold current heading
// this check is required to prevent bounce back after very fast yaw maneuvers // 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 // 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 target_yaw_rate = 0; // Stop rotation on yaw axis
// call attitude controller with target yaw rate = 0 to decelerate 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); 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 sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
} else { // call attitude controller holding absolute absolute bearing } 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);
} }
} }

45
ArduSub/control_stabilize.cpp
View File

@ -1,18 +1,16 @@
#include "Sub.h" #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; return true;
} }
// stabilize_run - runs the main stabilize controller void ModeStabilize::run()
// should be called at 100hz or more
void Sub::stabilize_run()
{ {
uint32_t tnow = AP_HAL::millis(); uint32_t tnow = AP_HAL::millis();
float target_roll, target_pitch; float target_roll, target_pitch;
@ -20,47 +18,48 @@ void Sub::stabilize_run()
// if not armed set throttle to zero and exit immediately // if not armed set throttle to zero and exit immediately
if (!motors.armed()) { if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control->relax_attitude_controllers();
last_pilot_heading = ahrs.yaw_sensor; sub.last_pilot_heading = ahrs.yaw_sensor;
return; return;
} }
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// convert pilot input to lean angles // convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats // To-Do: convert sub.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); // 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 // 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 // call attitude controller
// update attitude controller targets // update attitude controller targets
if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input 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); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
last_pilot_heading = ahrs.yaw_sensor; sub.last_pilot_heading = ahrs.yaw_sensor;
last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
} else { // hold current heading } else { // hold current heading
// this check is required to prevent bounce back after very fast yaw maneuvers // 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 // 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 target_yaw_rate = 0; // Stop rotation on yaw axis
// call attitude controller with target yaw rate = 0 to decelerate 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); 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 sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
} else { // call attitude controller holding absolute absolute bearing } 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 // 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 //control_in is range -1000-1000
//radio_in is raw pwm value //radio_in is raw pwm value

38
ArduSub/control_surface.cpp
View File

@ -1,24 +1,24 @@
#include "Sub.h" #include "Sub.h"
bool Sub::surface_init() bool ModeSurface::init(bool ignore_checks)
{ {
if(!control_check_barometer()) { if(!sub.control_check_barometer()) {
return false; return false;
} }
// initialize vertical speeds and acceleration // 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);
pos_control.set_correction_speed_accel_z(-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 // initialise position and desired velocity
pos_control.init_z_controller(); position_control->init_z_controller();
return true; return true;
} }
void Sub::surface_run() void ModeSurface::run()
{ {
float target_roll, target_pitch; float target_roll, target_pitch;
@ -26,36 +26,36 @@ void Sub::surface_run()
if (!motors.armed()) { if (!motors.armed()) {
motors.output_min(); motors.output_min();
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control->relax_attitude_controllers();
pos_control.init_z_controller(); position_control->init_z_controller();
return; return;
} }
// Already at surface, hold depth at surface // Already at surface, hold depth at surface
if (ap.at_surface) { if (sub.ap.at_surface) {
set_mode(ALT_HOLD, ModeReason::SURFACE_COMPLETE); set_mode(Mode::Number::ALT_HOLD, ModeReason::SURFACE_COMPLETE);
} }
// convert pilot input to lean angles // convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats // To-Do: convert sub.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); 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 // 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 // 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 // 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 // record desired climb rate for logging
desired_climb_rate = cmb_rate; sub.desired_climb_rate = cmb_rate;
// update altitude target and call position controller // update altitude target and call position controller
pos_control.set_pos_target_z_from_climb_rate_cm(cmb_rate); position_control->set_pos_target_z_from_climb_rate_cm(cmb_rate);
pos_control.update_z_controller(); position_control->update_z_controller();
// pilot has control for repositioning // pilot has control for repositioning
motors.set_forward(channel_forward->norm_input()); motors.set_forward(channel_forward->norm_input());

39
ArduSub/defines.h
View File

@ -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_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 // Acro Trainer types
#define ACRO_TRAINER_DISABLED 0 #define ACRO_TRAINER_DISABLED 0
#define ACRO_TRAINER_LEVELING 1 #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_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 #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. // Logging parameters - only 32 messages are available to the vehicle here.
enum LoggingParameters { enum LoggingParameters {

24
ArduSub/failsafe.cpp
View File

@ -88,9 +88,9 @@ void Sub::failsafe_sensors_check()
gcs().send_text(MAV_SEVERITY_CRITICAL, "Depth sensor error!"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Depth sensor error!");
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_SENSORS, LogErrorCode::BAD_DEPTH); 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 // 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 // We should never get here
arming.disarm(AP_Arming::Method::BADFLOWOFCONTROL); 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) { switch((Failsafe_Action)action) {
case Failsafe_Action_Surface: case Failsafe_Action_Surface:
set_mode(SURFACE, ModeReason::BATTERY_FAILSAFE); set_mode(Mode::Number::SURFACE, ModeReason::BATTERY_FAILSAFE);
break; break;
case Failsafe_Action_Disarm: case Failsafe_Action_Disarm:
arming.disarm(AP_Arming::Method::BATTERYFAILSAFE); arming.disarm(AP_Arming::Method::BATTERYFAILSAFE);
@ -299,7 +299,7 @@ void Sub::failsafe_leak_check()
// Handle failsafe action // Handle failsafe action
if (failsafe.leak && g.failsafe_leak == FS_LEAK_SURFACE && motors.armed()) { 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) { if (g.failsafe_gcs == FS_GCS_DISARM) {
arming.disarm(AP_Arming::Method::GCSFAILSAFE); arming.disarm(AP_Arming::Method::GCSFAILSAFE);
} else if (g.failsafe_gcs == FS_GCS_HOLD && motors.armed()) { } 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); arming.disarm(AP_Arming::Method::GCS_FAILSAFE_HOLDFAILED);
} }
} else if (g.failsafe_gcs == FS_GCS_SURFACE && motors.armed()) { } 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); 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 // 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; last_crash_check_pass_ms = tnow;
failsafe.crash = false; failsafe.crash = false;
return; return;
@ -425,7 +425,7 @@ void Sub::failsafe_crash_check()
void Sub::failsafe_terrain_check() void Sub::failsafe_terrain_check()
{ {
// trigger with 5 seconds of failures while in AUTO mode // 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 timeout = (failsafe.terrain_last_failure_ms - failsafe.terrain_first_failure_ms) > FS_TERRAIN_TIMEOUT_MS;
bool trigger_event = valid_mode && timeout; 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); AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED);
// If rangefinder is enabled, we can recover from this failsafe // 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(); failsafe_terrain_act();
} }
@ -482,14 +482,14 @@ void Sub::failsafe_terrain_act()
{ {
switch (g.failsafe_terrain) { switch (g.failsafe_terrain) {
case FS_TERRAIN_HOLD: case FS_TERRAIN_HOLD:
if (!set_mode(POSHOLD, ModeReason::TERRAIN_FAILSAFE)) { if (!set_mode(Mode::Number::POSHOLD, ModeReason::TERRAIN_FAILSAFE)) {
set_mode(ALT_HOLD, ModeReason::TERRAIN_FAILSAFE); set_mode(Mode::Number::ALT_HOLD, ModeReason::TERRAIN_FAILSAFE);
} }
AP_Notify::events.failsafe_mode_change = 1; AP_Notify::events.failsafe_mode_change = 1;
break; break;
case FS_TERRAIN_SURFACE: 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; AP_Notify::events.failsafe_mode_change = 1;
break; break;

2
ArduSub/fence.cpp
View File

@ -16,7 +16,7 @@ void Sub::fence_check()
const uint8_t orig_breaches = fence.get_breaches(); const uint8_t orig_breaches = fence.get_breaches();
// check for new breaches; new_breaches is bitmask of fence types breached // 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 there is a new breach take action
if (new_breaches) { if (new_breaches) {

230
ArduSub/flight_mode.cpp
View File

@ -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;
}
}

19
ArduSub/joystick.cpp
View File

@ -1,4 +1,5 @@
#include "Sub.h" #include "Sub.h"
#include "mode.h"
// Functions that will handle joystick/gamepad input // Functions that will handle joystick/gamepad input
// ---------------------------------------------------------------------------- // ----------------------------------------------------------------------------
@ -34,7 +35,7 @@ void Sub::init_joystick()
lights1 = RC_Channels::rc_channel(8)->get_radio_min(); lights1 = RC_Channels::rc_channel(8)->get_radio_min();
lights2 = RC_Channels::rc_channel(9)->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) { if (g.numGainSettings < 1) {
g.numGainSettings.set_and_save(1); g.numGainSettings.set_and_save(1);
@ -157,28 +158,28 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held)
break; break;
case JSButton::button_function_t::k_mode_manual: case JSButton::button_function_t::k_mode_manual:
set_mode(MANUAL, ModeReason::RC_COMMAND); set_mode(Mode::Number::MANUAL, ModeReason::RC_COMMAND);
break; break;
case JSButton::button_function_t::k_mode_stabilize: case JSButton::button_function_t::k_mode_stabilize:
set_mode(STABILIZE, ModeReason::RC_COMMAND); set_mode(Mode::Number::STABILIZE, ModeReason::RC_COMMAND);
break; break;
case JSButton::button_function_t::k_mode_depth_hold: 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; break;
case JSButton::button_function_t::k_mode_auto: case JSButton::button_function_t::k_mode_auto:
set_mode(AUTO, ModeReason::RC_COMMAND); set_mode(Mode::Number::AUTO, ModeReason::RC_COMMAND);
break; break;
case JSButton::button_function_t::k_mode_guided: case JSButton::button_function_t::k_mode_guided:
set_mode(GUIDED, ModeReason::RC_COMMAND); set_mode(Mode::Number::GUIDED, ModeReason::RC_COMMAND);
break; break;
case JSButton::button_function_t::k_mode_circle: case JSButton::button_function_t::k_mode_circle:
set_mode(CIRCLE, ModeReason::RC_COMMAND); set_mode(Mode::Number::CIRCLE, ModeReason::RC_COMMAND);
break; break;
case JSButton::button_function_t::k_mode_acro: case JSButton::button_function_t::k_mode_acro:
set_mode(ACRO, ModeReason::RC_COMMAND); set_mode(Mode::Number::ACRO, ModeReason::RC_COMMAND);
break; break;
case JSButton::button_function_t::k_mode_poshold: case JSButton::button_function_t::k_mode_poshold:
set_mode(POSHOLD, ModeReason::RC_COMMAND); set_mode(Mode::Number::POSHOLD, ModeReason::RC_COMMAND);
break; break;
case JSButton::button_function_t::k_mount_center: case JSButton::button_function_t::k_mount_center:

292
ArduSub/mode.cpp Normal file
View File

@ -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<Mode::Number>(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();
}

444
ArduSub/mode.h Normal file
View File

@ -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"; }
};

2
ArduSub/motors.cpp
View File

@ -10,7 +10,7 @@ void Sub::enable_motor_output()
void Sub::motors_output() void Sub::motors_output()
{ {
// Motor detection mode controls the thrusters directly // Motor detection mode controls the thrusters directly
if (control_mode == MOTOR_DETECT){ if (control_mode == Mode::Number::MOTOR_DETECT){
return; return;
} }
// check if we are performing the motor test // check if we are performing the motor test