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);
// log flight mode in case it was changed while vehicle was disarmed
AP::logger().Write_Mode(sub.control_mode, sub.control_mode_reason);
AP::logger().Write_Mode((uint8_t)sub.control_mode, sub.control_mode_reason);
// reenable failsafe
sub.mainloop_failsafe_enable();

4
ArduSub/ArduSub.cpp
View File

@ -140,7 +140,7 @@ void Sub::run_rate_controller()
pos_control.set_dt(last_loop_time_s);
//don't run rate controller in manual or motordetection modes
if (control_mode != MANUAL && control_mode != MOTOR_DETECT) {
if (control_mode != Mode::Number::MANUAL && control_mode != Mode::Number::MOTOR_DETECT) {
// run low level rate controllers that only require IMU data and set loop time
attitude_control.rate_controller_run();
}
@ -201,7 +201,7 @@ void Sub::ten_hz_logging_loop()
if (should_log(MASK_LOG_RCOUT)) {
logger.Write_RCOUT();
}
if (should_log(MASK_LOG_NTUN) && (mode_requires_GPS(control_mode) || !mode_has_manual_throttle(control_mode))) {
if (should_log(MASK_LOG_NTUN) && (sub.flightmode->requires_GPS() || !sub.flightmode->has_manual_throttle())) {
pos_control.write_log();
}
if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {

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
// ArduPlane documentation
switch (sub.control_mode) {
case AUTO:
case GUIDED:
case CIRCLE:
case POSHOLD:
case Mode::Number::AUTO:
case Mode::Number::GUIDED:
case Mode::Number::CIRCLE:
case Mode::Number::POSHOLD:
_base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
// APM does in any mode, as that is defined as "system finds its own goal
@ -50,7 +50,7 @@ MAV_MODE GCS_MAVLINK_Sub::base_mode() const
uint32_t GCS_Sub::custom_mode() const
{
return sub.control_mode;
return (uint32_t)sub.control_mode;
}
MAV_STATE GCS_MAVLINK_Sub::vehicle_system_status() const
@ -448,7 +448,7 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_do_set_roi(const Location &roi_loc)
if (!roi_loc.check_latlng()) {
return MAV_RESULT_FAILED;
}
sub.set_auto_yaw_roi(roi_loc);
sub.mode_auto.set_auto_yaw_roi(roi_loc);
return MAV_RESULT_ACCEPTED;
}
@ -464,13 +464,13 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon
{
switch (packet.command) {
case MAV_CMD_NAV_LOITER_UNLIM:
if (!sub.set_mode(POSHOLD, ModeReason::GCS_COMMAND)) {
if (!sub.set_mode(Mode::Number::POSHOLD, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
case MAV_CMD_NAV_LAND:
if (!sub.set_mode(SURFACE, ModeReason::GCS_COMMAND)) {
if (!sub.set_mode(Mode::Number::SURFACE, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
@ -483,7 +483,7 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon
if ((packet.param1 >= 0.0f) &&
(packet.param1 <= 360.0f) &&
(is_zero(packet.param4) || is_equal(packet.param4,1.0f))) {
sub.set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, (uint8_t)packet.param4);
sub.mode_auto.set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, (uint8_t)packet.param4);
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
@ -500,7 +500,7 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon
return MAV_RESULT_FAILED;
case MAV_CMD_MISSION_START:
if (sub.motors.armed() && sub.set_mode(AUTO, ModeReason::GCS_COMMAND)) {
if (sub.motors.armed() && sub.set_mode(Mode::Number::AUTO, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
@ -588,7 +588,7 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg)
// descend at up to WPNAV_SPEED_DN
climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * sub.wp_nav.get_default_speed_down();
}
sub.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms);
sub.mode_guided.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms);
break;
}
@ -598,7 +598,7 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg)
mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
// exit if vehicle is not in Guided mode or Auto-Guided mode
if ((sub.control_mode != GUIDED) && !(sub.control_mode == AUTO && sub.auto_mode == Auto_NavGuided)) {
if ((sub.control_mode != Mode::Number::GUIDED) && !(sub.control_mode == Mode::Number::AUTO && sub.auto_mode == Auto_NavGuided)) {
break;
}
@ -652,11 +652,11 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg)
// send request
if (!pos_ignore && !vel_ignore && acc_ignore) {
sub.guided_set_destination_posvel(pos_vector, vel_vector);
sub.mode_guided.guided_set_destination_posvel(pos_vector, vel_vector);
} else if (pos_ignore && !vel_ignore && acc_ignore) {
sub.guided_set_velocity(vel_vector);
sub.mode_guided.guided_set_velocity(vel_vector);
} else if (!pos_ignore && vel_ignore && acc_ignore) {
sub.guided_set_destination(pos_vector);
sub.mode_guided.guided_set_destination(pos_vector);
}
break;
@ -668,9 +668,9 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg)
mavlink_msg_set_position_target_global_int_decode(&msg, &packet);
// exit if vehicle is not in Guided, Auto-Guided, or Depth Hold modes
if ((sub.control_mode != GUIDED)
&& !(sub.control_mode == AUTO && sub.auto_mode == Auto_NavGuided)
&& !(sub.control_mode == ALT_HOLD)) {
if ((sub.control_mode != Mode::Number::GUIDED)
&& !(sub.control_mode == Mode::Number::AUTO && sub.auto_mode == Auto_NavGuided)
&& !(sub.control_mode == Mode::Number::ALT_HOLD)) {
break;
}
@ -686,7 +686,7 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg)
* bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
*/
if (!z_ignore && sub.control_mode == ALT_HOLD) { // Control only target depth when in ALT_HOLD
if (!z_ignore && sub.control_mode == Mode::Number::ALT_HOLD) { // Control only target depth when in ALT_HOLD
sub.pos_control.set_pos_target_z_cm(packet.alt*100);
break;
}
@ -715,11 +715,11 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg)
}
if (!pos_ignore && !vel_ignore && acc_ignore) {
sub.guided_set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
sub.mode_guided.guided_set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
} else if (pos_ignore && !vel_ignore && acc_ignore) {
sub.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
sub.mode_guided.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
} else if (!pos_ignore && vel_ignore && acc_ignore) {
sub.guided_set_destination(pos_neu_cm);
sub.mode_guided.guided_set_destination(pos_neu_cm);
}
break;
@ -794,7 +794,7 @@ int16_t GCS_MAVLINK_Sub::high_latency_target_altitude() const
UNUSED_RESULT(ahrs.get_location(global_position_current));
//return units are m
if (sub.control_mode == AUTO || sub.control_mode == GUIDED) {
if (sub.control_mode == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) {
return 0.01 * (global_position_current.alt + sub.pos_control.get_pos_error_z_cm());
}
return 0;
@ -804,7 +804,7 @@ int16_t GCS_MAVLINK_Sub::high_latency_target_altitude() const
uint8_t GCS_MAVLINK_Sub::high_latency_tgt_heading() const
{
// return units are deg/2
if (sub.control_mode == AUTO || sub.control_mode == GUIDED) {
if (sub.control_mode == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) {
// need to convert -18000->18000 to 0->360/2
return wrap_360_cd(sub.wp_nav.get_wp_bearing_to_destination()) / 200;
}
@ -814,7 +814,7 @@ uint8_t GCS_MAVLINK_Sub::high_latency_tgt_heading() const
uint16_t GCS_MAVLINK_Sub::high_latency_tgt_dist() const
{
// return units are dm
if (sub.control_mode == AUTO || sub.control_mode == GUIDED) {
if (sub.control_mode == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) {
return MIN(sub.wp_nav.get_wp_distance_to_destination() * 0.001, UINT16_MAX);
}
return 0;
@ -823,7 +823,7 @@ uint16_t GCS_MAVLINK_Sub::high_latency_tgt_dist() const
uint8_t GCS_MAVLINK_Sub::high_latency_tgt_airspeed() const
{
// return units are m/s*5
if (sub.control_mode == AUTO || sub.control_mode == GUIDED) {
if (sub.control_mode == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) {
return MIN((sub.pos_control.get_vel_desired_cms().length()/100) * 5, UINT8_MAX);
}
return 0;

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;
switch (sub.control_mode) {
case ALT_HOLD:
case AUTO:
case GUIDED:
case CIRCLE:
case SURFACE:
case POSHOLD:
case Mode::Number::ALT_HOLD:
case Mode::Number::AUTO:
case Mode::Number::GUIDED:
case Mode::Number::CIRCLE:
case Mode::Number::SURFACE:
case Mode::Number::POSHOLD:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

2
ArduSub/Log.cpp
View File

@ -278,7 +278,7 @@ const struct LogStructure Sub::log_structure[] = {
void Sub::Log_Write_Vehicle_Startup_Messages()
{
// only 200(?) bytes are guaranteed by AP_Logger
logger.Write_Mode(control_mode, control_mode_reason);
logger.Write_Mode((uint8_t)control_mode, control_mode_reason);
ahrs.Log_Write_Home_And_Origin();
gps.Write_AP_Logger_Log_Startup_messages();
}

5
ArduSub/Sub.cpp
View File

@ -25,7 +25,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
*/
Sub::Sub()
: logger(g.log_bitmask),
control_mode(MANUAL),
control_mode(Mode::Number::MANUAL),
motors(MAIN_LOOP_RATE),
auto_mode(Auto_WP),
guided_mode(Guided_WP),
@ -37,7 +37,8 @@ Sub::Sub()
wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control),
loiter_nav(inertial_nav, ahrs_view, pos_control, attitude_control),
circle_nav(inertial_nav, ahrs_view, pos_control),
param_loader(var_info)
param_loader(var_info),
flightmode(&mode_manual)
{
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL
failsafe.pilot_input = true;

108
ArduSub/Sub.h
View File

@ -71,6 +71,7 @@
#include "Parameters.h"
#include "AP_Arming_Sub.h"
#include "GCS_Sub.h"
#include "mode.h"
#include <AP_OpticalFlow/AP_OpticalFlow.h> // Optical Flow library
@ -108,6 +109,17 @@ public:
friend class ParametersG2;
friend class AP_Arming_Sub;
friend class RC_Channels_Sub;
friend class Mode;
friend class ModeManual;
friend class ModeStabilize;
friend class ModeAcro;
friend class ModeAlthold;
friend class ModeGuided;
friend class ModePoshold;
friend class ModeAuto;
friend class ModeCircle;
friend class ModeSurface;
friend class ModeMotordetect;
Sub(void);
@ -190,9 +202,9 @@ private:
// This is the state of the flight control system
// There are multiple states defined such as STABILIZE, ACRO,
control_mode_t control_mode;
Mode::Number control_mode;
control_mode_t prev_control_mode;
Mode::Number prev_control_mode;
#if RCMAP_ENABLED == ENABLED
RCMapper rcmap;
@ -234,12 +246,6 @@ private:
AP_Motors6DOF motors;
// Auto
AutoMode auto_mode; // controls which auto controller is run
// Guided
GuidedMode guided_mode; // controls which controller is run (pos or vel)
// Circle
bool circle_pilot_yaw_override; // true if pilot is overriding yaw
@ -416,61 +422,7 @@ private:
bool verify_wait_delay();
bool verify_within_distance();
bool verify_yaw();
bool acro_init(void);
void acro_run();
void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
bool althold_init(void);
void althold_run();
bool auto_init(void);
void auto_run();
void auto_wp_start(const Vector3f& destination);
void auto_wp_start(const Location& dest_loc);
void auto_wp_run();
void auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn);
void auto_circle_start();
void auto_circle_run();
void auto_nav_guided_start();
void auto_nav_guided_run();
bool auto_loiter_start();
void auto_loiter_run();
uint8_t get_default_auto_yaw_mode(bool rtl) const;
void set_auto_yaw_mode(uint8_t yaw_mode);
void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle);
void set_auto_yaw_roi(const Location &roi_location);
float get_auto_heading(void);
bool circle_init(void);
void circle_run();
bool guided_init(bool ignore_checks = false);
void guided_pos_control_start();
void guided_vel_control_start();
void guided_posvel_control_start();
void guided_angle_control_start();
bool guided_set_destination(const Vector3f& destination);
bool guided_set_destination(const Location& dest_loc);
void guided_set_velocity(const Vector3f& velocity);
bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity);
void guided_set_angle(const Quaternion &q, float climb_rate_cms);
void guided_run();
void guided_pos_control_run();
void guided_vel_control_run();
void guided_posvel_control_run();
void guided_angle_control_run();
void guided_limit_clear();
void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);
void guided_limit_init_time_and_pos();
bool guided_limit_check();
bool poshold_init(void);
void poshold_run();
bool motordetect_init();
void motordetect_run();
bool stabilize_init(void);
void stabilize_run();
void control_depth();
bool manual_init(void);
void manual_run();
void failsafe_sensors_check(void);
void failsafe_crash_check();
void failsafe_ekf_check(void);
@ -484,15 +436,12 @@ private:
void mainloop_failsafe_enable();
void mainloop_failsafe_disable();
void fence_check();
bool set_mode(control_mode_t mode, ModeReason reason);
bool set_mode(const uint8_t mode, const ModeReason reason) override;
bool set_mode(Mode::Number mode, ModeReason reason);
bool set_mode(const uint8_t new_mode, const ModeReason reason) override;
uint8_t get_mode() const override { return (uint8_t)control_mode; }
void update_flight_mode();
void exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode);
bool mode_requires_GPS(control_mode_t mode);
bool mode_has_manual_throttle(control_mode_t mode);
bool mode_allows_arming(control_mode_t mode, bool arming_from_gcs);
void notify_flight_mode(control_mode_t mode);
void exit_mode(Mode::Number old_control_mode, Mode::Number new_control_mode);
void notify_flight_mode();
void read_inertia();
void update_surface_and_bottom_detector();
void set_surfaced(bool at_surface);
@ -563,8 +512,7 @@ private:
void failsafe_internal_temperature_check();
void failsafe_terrain_act(void);
bool auto_terrain_recover_start(void);
void auto_terrain_recover_run(void);
void translate_wpnav_rp(float &lateral_out, float &forward_out);
void translate_circle_nav_rp(float &lateral_out, float &forward_out);
@ -610,6 +558,24 @@ private:
static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
"_failsafe_priorities is missing the sentinel");
Mode *mode_from_mode_num(const Mode::Number num);
void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode);
Mode *flightmode;
ModeManual mode_manual;
ModeStabilize mode_stabilize;
ModeAcro mode_acro;
ModeAlthold mode_althold;
ModeAuto mode_auto;
ModeGuided mode_guided;
ModePoshold mode_poshold;
ModeCircle mode_circle;
ModeSurface mode_surface;
ModeMotordetect mode_motordetect;
// Auto
AutoSubMode auto_mode; // controls which auto controller is run
GuidedSubMode guided_mode;
public:
void mainloop_failsafe_check();

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()
bool Sub::verify_command_callback(const AP_Mission::Mission_Command& cmd)
{
if (control_mode == AUTO) {
if (control_mode == Mode::Number::AUTO) {
bool cmd_complete = verify_command(cmd);
// send message to GCS
@ -207,8 +207,8 @@ void Sub::exit_mission()
AP_Notify::events.mission_complete = 1;
// Try to enter loiter, if that fails, go to depth hold
if (!auto_loiter_start()) {
set_mode(ALT_HOLD, ModeReason::MISSION_END);
if (!mode_auto.auto_loiter_start()) {
set_mode(Mode::Number::ALT_HOLD, ModeReason::MISSION_END);
}
}
@ -243,7 +243,7 @@ void Sub::do_nav_wp(const AP_Mission::Mission_Command& cmd)
loiter_time_max = cmd.p1;
// Set wp navigation target
auto_wp_start(target_loc);
mode_auto.auto_wp_start(target_loc);
}
// do_surface - initiate surface procedure
@ -279,12 +279,12 @@ void Sub::do_surface(const AP_Mission::Mission_Command& cmd)
}
// Go to wp location
auto_wp_start(target_location);
mode_auto.auto_wp_start(target_location);
}
void Sub::do_RTL()
{
auto_wp_start(ahrs.get_home());
mode_auto.auto_wp_start(ahrs.get_home());
}
// do_loiter_unlimited - start loitering with no end conditions
@ -323,7 +323,7 @@ void Sub::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
}
// start way point navigator and provide it the desired location
auto_wp_start(target_loc);
mode_auto.auto_wp_start(target_loc);
}
// do_circle - initiate moving in a circle
@ -362,7 +362,7 @@ void Sub::do_circle(const AP_Mission::Mission_Command& cmd)
const bool circle_direction_ccw = cmd.content.location.loiter_ccw;
// move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
auto_circle_movetoedge_start(circle_center, circle_radius_m, circle_direction_ccw);
mode_auto.auto_circle_movetoedge_start(circle_center, circle_radius_m, circle_direction_ccw);
}
// do_loiter_time - initiate loitering at a point for a given time period
@ -383,10 +383,10 @@ void Sub::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
{
if (cmd.p1 > 0) {
// initialise guided limits
guided_limit_init_time_and_pos();
mode_auto.guided_limit_init_time_and_pos();
// set navigation target
auto_nav_guided_start();
mode_auto.auto_nav_guided_start();
}
}
#endif // NAV_GUIDED
@ -410,7 +410,7 @@ void Sub::do_nav_delay(const AP_Mission::Mission_Command& cmd)
// do_guided_limits - pass guided limits to guided controller
void Sub::do_guided_limits(const AP_Mission::Mission_Command& cmd)
{
guided_limit_set(cmd.p1 * 1000, // convert seconds to ms
mode_guided.guided_limit_set(cmd.p1 * 1000, // convert seconds to ms
cmd.content.guided_limits.alt_min * 100.0f, // convert meters to cm
cmd.content.guided_limits.alt_max * 100.0f, // convert meters to cm
cmd.content.guided_limits.horiz_max * 100.0f); // convert meters to cm
@ -459,7 +459,7 @@ bool Sub::verify_surface(const AP_Mission::Mission_Command& cmd)
// TODO get xy target from current wp destination, because current location may be acceptance-radius away from original destination
Location target_location(cmd.content.location.lat, cmd.content.location.lng, 0, Location::AltFrame::ABOVE_HOME);
auto_wp_start(target_location);
mode_auto.auto_wp_start(target_location);
// advance to next state
auto_surface_state = AUTO_SURFACE_STATE_ASCEND;
@ -529,13 +529,13 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)
}
// start circling
auto_circle_start();
mode_auto.auto_circle_start();
}
return false;
}
// check if we have completed circling
return fabsf(circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
return fabsf(sub.circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
}
#if NAV_GUIDED == ENABLED
@ -548,7 +548,7 @@ bool Sub::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
}
// check time and position limits
return guided_limit_check();
return mode_auto.guided_limit_check();
}
#endif // NAV_GUIDED
@ -579,7 +579,7 @@ void Sub::do_within_distance(const AP_Mission::Mission_Command& cmd)
void Sub::do_yaw(const AP_Mission::Mission_Command& cmd)
{
set_auto_yaw_look_at_heading(
sub.mode_auto.set_auto_yaw_look_at_heading(
cmd.content.yaw.angle_deg,
cmd.content.yaw.turn_rate_dps,
cmd.content.yaw.direction,
@ -614,7 +614,7 @@ bool Sub::verify_yaw()
{
// set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command)
if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) {
set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
sub.mode_auto.set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
}
// check if we are within 2 degrees of the target heading
@ -629,7 +629,7 @@ bool Sub::verify_yaw()
bool Sub::do_guided(const AP_Mission::Mission_Command& cmd)
{
// only process guided waypoint if we are in guided mode
if (control_mode != GUIDED && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) {
if (control_mode != Mode::Number::GUIDED && !(control_mode == Mode::Number::AUTO && auto_mode == Auto_NavGuided)) {
return false;
}
@ -638,7 +638,7 @@ bool Sub::do_guided(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_NAV_WAYPOINT: {
// set wp_nav's destination
return guided_set_destination(cmd.content.location);
return sub.mode_guided.guided_set_destination(cmd.content.location);
}
case MAV_CMD_CONDITION_YAW:
@ -681,7 +681,7 @@ void Sub::do_set_home(const AP_Mission::Mission_Command& cmd)
// TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint
void Sub::do_roi(const AP_Mission::Mission_Command& cmd)
{
set_auto_yaw_roi(cmd.content.location);
sub.mode_auto.set_auto_yaw_roi(cmd.content.location);
}
// point the camera to a specified angle

138
ArduSub/control_acro.cpp
View File

@ -1,34 +1,30 @@
#include "Sub.h"
/*
* control_acro.pde - init and run calls for acro flight mode
*/
// acro_init - initialise acro controller
bool Sub::acro_init()
{
#include "Sub.h"
bool ModeAcro::init(bool ignore_checks) {
// set target altitude to zero for reporting
pos_control.set_pos_target_z_cm(0);
position_control->set_pos_target_z_cm(0);
// attitude hold inputs become thrust inputs in acro mode
// set to neutral to prevent chaotic behavior (esp. roll/pitch)
set_neutral_controls();
sub.set_neutral_controls();
return true;
}
// acro_run - runs the acro controller
// should be called at 100hz or more
void Sub::acro_run()
void ModeAcro::run()
{
float target_roll, target_pitch, target_yaw;
// if not armed set throttle to zero and exit immediately
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control->relax_attitude_controllers();
return;
}
@ -38,125 +34,13 @@ void Sub::acro_run()
get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw);
// run attitude controller
attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
// output pilot's throttle without angle boost
attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);
attitude_control->set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);
//control_in is range 0-1000
//radio_in is raw pwm value
motors.set_forward(channel_forward->norm_input());
motors.set_lateral(channel_lateral->norm_input());
}
// get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates
// returns desired angle rates in centi-degrees-per-second
void Sub::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
{
float rate_limit;
Vector3f rate_ef_level, rate_bf_level, rate_bf_request;
// apply circular limit to pitch and roll inputs
float total_in = norm(pitch_in, roll_in);
if (total_in > ROLL_PITCH_INPUT_MAX) {
float ratio = (float)ROLL_PITCH_INPUT_MAX / total_in;
roll_in *= ratio;
pitch_in *= ratio;
}
// calculate roll, pitch rate requests
if (g.acro_expo <= 0) {
rate_bf_request.x = roll_in * g.acro_rp_p;
rate_bf_request.y = pitch_in * g.acro_rp_p;
} else {
// expo variables
float rp_in, rp_in3, rp_out;
// range check expo
if (g.acro_expo > 1.0f) {
g.acro_expo.set(1.0f);
}
// roll expo
rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX;
rp_in3 = rp_in*rp_in*rp_in;
rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in);
rate_bf_request.x = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
// pitch expo
rp_in = float(pitch_in)/ROLL_PITCH_INPUT_MAX;
rp_in3 = rp_in*rp_in*rp_in;
rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in);
rate_bf_request.y = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
}
// calculate yaw rate request
rate_bf_request.z = yaw_in * g.acro_yaw_p;
// calculate earth frame rate corrections to pull the vehicle back to level while in ACRO mode
if (g.acro_trainer != ACRO_TRAINER_DISABLED) {
// Calculate trainer mode earth frame rate command for roll
int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;
// Calculate trainer mode earth frame rate command for pitch
int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);
rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
// Calculate trainer mode earth frame rate command for yaw
rate_ef_level.z = 0;
// Calculate angle limiting earth frame rate commands
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
if (roll_angle > aparm.angle_max) {
rate_ef_level.x -= g.acro_balance_roll*(roll_angle-aparm.angle_max);
} else if (roll_angle < -aparm.angle_max) {
rate_ef_level.x -= g.acro_balance_roll*(roll_angle+aparm.angle_max);
}
if (pitch_angle > aparm.angle_max) {
rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle-aparm.angle_max);
} else if (pitch_angle < -aparm.angle_max) {
rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle+aparm.angle_max);
}
}
// convert earth-frame level rates to body-frame level rates
attitude_control.euler_rate_to_ang_vel(attitude_control.get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level);
// combine earth frame rate corrections with rate requests
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
rate_bf_request.x += rate_bf_level.x;
rate_bf_request.y += rate_bf_level.y;
rate_bf_request.z += rate_bf_level.z;
} else {
float acro_level_mix = constrain_float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch();
// Scale leveling rates by stick input
rate_bf_level = rate_bf_level*acro_level_mix;
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabsf(fabsf(rate_bf_request.x)-fabsf(rate_bf_level.x));
rate_bf_request.x += rate_bf_level.x;
rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit);
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabsf(fabsf(rate_bf_request.y)-fabsf(rate_bf_level.y));
rate_bf_request.y += rate_bf_level.y;
rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit);
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabsf(fabsf(rate_bf_request.z)-fabsf(rate_bf_level.z));
rate_bf_request.z += rate_bf_level.z;
rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit);
}
}
// hand back rate request
roll_out = rate_bf_request.x;
pitch_out = rate_bf_request.y;
yaw_out = rate_bf_request.z;
}

70
ArduSub/control_althold.cpp
View File

@ -1,46 +1,40 @@
#include "Sub.h"
/*
* control_althold.pde - init and run calls for althold, flight mode
*/
// althold_init - initialise althold controller
bool Sub::althold_init()
{
if(!control_check_barometer()) {
bool ModeAlthold::init(bool ignore_checks) {
if(!sub.control_check_barometer()) {
return false;
}
// initialize vertical maximum speeds and acceleration
// sets the maximum speed up and down returned by position controller
pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
// initialise position and desired velocity
pos_control.init_z_controller();
position_control->init_z_controller();
last_pilot_heading = ahrs.yaw_sensor;
sub.last_pilot_heading = ahrs.yaw_sensor;
return true;
}
// althold_run - runs the althold controller
// should be called at 100hz or more
void Sub::althold_run()
void ModeAlthold::run()
{
uint32_t tnow = AP_HAL::millis();
// initialize vertical speeds and acceleration
pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
// Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
attitude_control.set_throttle_out(0.5,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
pos_control.relax_z_controller(motors.get_throttle_hover());
last_pilot_heading = ahrs.yaw_sensor;
attitude_control->set_throttle_out(0.5,true,g.throttle_filt);
attitude_control->relax_attitude_controllers();
position_control->relax_z_controller(motors.get_throttle_hover());
sub.last_pilot_heading = ahrs.yaw_sensor;
return;
}
@ -53,7 +47,7 @@ void Sub::althold_run()
if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
float target_yaw;
Quaternion(
set_attitude_target_no_gps.packet.q
sub.set_attitude_target_no_gps.packet.q
).to_euler(
target_roll,
target_pitch,
@ -63,34 +57,34 @@ void Sub::althold_run()
target_pitch = degrees(target_pitch);
target_yaw = degrees(target_yaw);
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true);
attitude_control->input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true);
return;
}
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max_cd());
sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->get_althold_lean_angle_max_cd());
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// call attitude controller
if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
last_pilot_heading = ahrs.yaw_sensor;
last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
sub.last_pilot_heading = ahrs.yaw_sensor;
sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
} else { // hold current heading
// this check is required to prevent bounce back after very fast yaw maneuvers
// the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
if (tnow < sub.last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
target_yaw_rate = 0; // Stop rotation on yaw axis
// call attitude controller with target yaw rate = 0 to decelerate on yaw axis
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
} else { // call attitude controller holding absolute absolute bearing
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true);
attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, sub.last_pilot_heading, true);
}
}
@ -100,21 +94,21 @@ void Sub::althold_run()
motors.set_lateral(channel_lateral->norm_input());
}
void Sub::control_depth() {
float target_climb_rate_cm_s = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
target_climb_rate_cm_s = constrain_float(target_climb_rate_cm_s, -get_pilot_speed_dn(), g.pilot_speed_up);
void ModeAlthold::control_depth() {
float target_climb_rate_cm_s = sub.get_pilot_desired_climb_rate(channel_throttle->get_control_in());
target_climb_rate_cm_s = constrain_float(target_climb_rate_cm_s, -sub.get_pilot_speed_dn(), g.pilot_speed_up);
// desired_climb_rate returns 0 when within the deadzone.
//we allow full control to the pilot, but as soon as there's no input, we handle being at surface/bottom
if (fabsf(target_climb_rate_cm_s) < 0.05f) {
if (ap.at_surface) {
pos_control.set_pos_target_z_cm(MIN(pos_control.get_pos_target_z_cm(), g.surface_depth - 5.0f)); // set target to 5 cm below surface level
} else if (ap.at_bottom) {
pos_control.set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, pos_control.get_pos_target_z_cm())); // set target to 10 cm above bottom
if (sub.ap.at_surface) {
position_control->set_pos_target_z_cm(MIN(position_control->get_pos_target_z_cm(), g.surface_depth - 5.0f)); // set target to 5 cm below surface level
} else if (sub.ap.at_bottom) {
position_control->set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_target_z_cm())); // set target to 10 cm above bottom
}
}
pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s);
pos_control.update_z_controller();
position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s);
position_control->update_z_controller();
}

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.
* Code in this file implements the navigation commands
*/
// auto_init - initialise auto controller
bool Sub::auto_init()
{
if (!position_ok() || mission.num_commands() < 2) {
bool ModeAuto::init(bool ignore_checks) {
if (!sub.position_ok() || sub.mission.num_commands() < 2) {
return false;
}
auto_mode = Auto_Loiter;
sub.auto_mode = Auto_Loiter;
// stop ROI from carrying over from previous runs of the mission
// To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
if (auto_yaw_mode == AUTO_YAW_ROI) {
if (sub.auto_yaw_mode == AUTO_YAW_ROI) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// initialise waypoint controller
wp_nav.wp_and_spline_init();
sub.wp_nav.wp_and_spline_init();
// clear guided limits
guided_limit_clear();
// start/resume the mission (based on MIS_RESTART parameter)
mission.start_or_resume();
sub.mission.start_or_resume();
return true;
}
// auto_run - runs the appropriate auto controller
// according to the current auto_mode
// should be called at 100hz or more
void Sub::auto_run()
void ModeAuto::run()
{
mission.update();
sub.mission.update();
// call the correct auto controller
switch (auto_mode) {
switch (sub.auto_mode) {
case Auto_WP:
case Auto_CircleMoveToEdge:
@ -70,42 +66,42 @@ void Sub::auto_run()
}
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
void Sub::auto_wp_start(const Vector3f& destination)
void ModeAuto::auto_wp_start(const Vector3f& destination)
{
auto_mode = Auto_WP;
sub.auto_mode = Auto_WP;
// initialise wpnav (no need to check return status because terrain data is not used)
wp_nav.set_wp_destination(destination, false);
sub.wp_nav.set_wp_destination(destination, false);
// initialise yaw
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
if (auto_yaw_mode != AUTO_YAW_ROI) {
if (sub.auto_yaw_mode != AUTO_YAW_ROI) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
}
}
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
void Sub::auto_wp_start(const Location& dest_loc)
void ModeAuto::auto_wp_start(const Location& dest_loc)
{
auto_mode = Auto_WP;
sub.auto_mode = Auto_WP;
// send target to waypoint controller
if (!wp_nav.set_wp_destination_loc(dest_loc)) {
if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) {
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event();
sub.failsafe_terrain_on_event();
return;
}
// initialise yaw
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
if (auto_yaw_mode != AUTO_YAW_ROI) {
if (sub.auto_yaw_mode != AUTO_YAW_ROI) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
}
}
// auto_wp_run - runs the auto waypoint controller
// called by auto_run at 100hz or more
void Sub::auto_wp_run()
void ModeAuto::auto_wp_run()
{
// if not armed set throttle to zero and exit immediately
if (!motors.armed()) {
@ -114,17 +110,17 @@ void Sub::auto_wp_run()
// call attitude controller
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
wp_nav.wp_and_spline_init(); // Reset xy target
attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control->relax_attitude_controllers();
sub.wp_nav.wp_and_spline_init(); // Reset xy target
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.pilot_input) {
if (!sub.failsafe.pilot_input) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
@ -137,13 +133,13 @@ void Sub::auto_wp_run()
// TODO logic for terrain tracking target going below fence limit
// TODO implement waypoint radius individually for each waypoint based on cmd.p2
// TODO fix auto yaw heading to switch to something appropriate when mission complete and switches to loiter
failsafe_terrain_set_status(wp_nav.update_wpnav());
sub.failsafe_terrain_set_status(sub.wp_nav.update_wpnav());
///////////////////////
// update xy outputs //
float lateral_out, forward_out;
translate_wpnav_rp(lateral_out, forward_out);
sub.translate_wpnav_rp(lateral_out, forward_out);
// Send to forward/lateral outputs
motors.set_lateral(lateral_out);
@ -151,52 +147,52 @@ void Sub::auto_wp_run()
// WP_Nav has set the vertical position control targets
// run the vertical position controller and set output throttle
pos_control.update_z_controller();
position_control->update_z_controller();
////////////////////////////
// update attitude output //
// get pilot desired lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max);
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
if (sub.auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch & yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
} else {
// roll, pitch from pilot, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true);
attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true);
}
}
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
// we assume the caller has set the circle's circle with circle_nav.set_center()
// we assume the caller has set the circle's circle with sub.circle_nav.set_center()
// we assume the caller has performed all required GPS_ok checks
void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn)
void ModeAuto::auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn)
{
// set circle center
circle_nav.set_center(circle_center);
sub.circle_nav.set_center(circle_center);
// set circle radius
if (!is_zero(radius_m)) {
circle_nav.set_radius_cm(radius_m * 100.0f);
sub.circle_nav.set_radius_cm(radius_m * 100.0f);
}
// set circle direction by using rate
float current_rate = circle_nav.get_rate();
float current_rate = sub.circle_nav.get_rate();
current_rate = ccw_turn ? -fabsf(current_rate) : fabsf(current_rate);
circle_nav.set_rate(current_rate);
sub.circle_nav.set_rate(current_rate);
// check our distance from edge of circle
Vector3f circle_edge_neu;
circle_nav.get_closest_point_on_circle(circle_edge_neu);
sub.circle_nav.get_closest_point_on_circle(circle_edge_neu);
float dist_to_edge = (inertial_nav.get_position_neu_cm() - circle_edge_neu).length();
// if more than 3m then fly to edge
if (dist_to_edge > 300.0f) {
// set the state to move to the edge of the circle
auto_mode = Auto_CircleMoveToEdge;
sub.auto_mode = Auto_CircleMoveToEdge;
// convert circle_edge_neu to Location
Location circle_edge(circle_edge_neu, Location::AltFrame::ABOVE_ORIGIN);
@ -205,14 +201,14 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi
circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame());
// initialise wpnav to move to edge of circle
if (!wp_nav.set_wp_destination_loc(circle_edge)) {
if (!sub.wp_nav.set_wp_destination_loc(circle_edge)) {
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event();
sub.failsafe_terrain_on_event();
}
// if we are outside the circle, point at the edge, otherwise hold yaw
float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy_cm().topostype(), circle_nav.get_center().xy());
if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {
float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy_cm().topostype(), sub.circle_nav.get_center().xy());
if (dist_to_center > sub.circle_nav.get_radius() && dist_to_center > 500) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
} else {
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
@ -225,23 +221,23 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi
// auto_circle_start - initialises controller to fly a circle in AUTO flight mode
// assumes that circle_nav object has already been initialised with circle center and radius
void Sub::auto_circle_start()
void ModeAuto::auto_circle_start()
{
auto_mode = Auto_Circle;
sub.auto_mode = Auto_Circle;
// initialise circle controller
circle_nav.init(circle_nav.get_center(), circle_nav.center_is_terrain_alt(), circle_nav.get_rate());
sub.circle_nav.init(sub.circle_nav.get_center(), sub.circle_nav.center_is_terrain_alt(), sub.circle_nav.get_rate());
}
// auto_circle_run - circle in AUTO flight mode
// called by auto_run at 100hz or more
void Sub::auto_circle_run()
void ModeAuto::auto_circle_run()
{
// call circle controller
failsafe_terrain_set_status(circle_nav.update());
sub.failsafe_terrain_set_status(sub.circle_nav.update());
float lateral_out, forward_out;
translate_circle_nav_rp(lateral_out, forward_out);
sub.translate_circle_nav_rp(lateral_out, forward_out);
// Send to forward/lateral outputs
motors.set_lateral(lateral_out);
@ -249,50 +245,47 @@ void Sub::auto_circle_run()
// WP_Nav has set the vertical position control targets
// run the vertical position controller and set output throttle
pos_control.update_z_controller();
position_control->update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true);
attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), sub.circle_nav.get_yaw(), true);
}
#if NAV_GUIDED == ENABLED
// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
void Sub::auto_nav_guided_start()
void ModeAuto::auto_nav_guided_start()
{
auto_mode = Auto_NavGuided;
// call regular guided flight mode initialisation
guided_init(true);
sub.mode_guided.init(true);
sub.auto_mode = Auto_NavGuided;
// initialise guided start time and position as reference for limit checking
guided_limit_init_time_and_pos();
sub.mode_auto.guided_limit_init_time_and_pos();
}
// auto_nav_guided_run - allows control by external navigation controller
// called by auto_run at 100hz or more
void Sub::auto_nav_guided_run()
void ModeAuto::auto_nav_guided_run()
{
// call regular guided flight mode run function
guided_run();
sub.mode_guided.run();
}
#endif // NAV_GUIDED
// auto_loiter_start - initialises loitering in auto mode
// returns success/failure because this can be called by exit_mission
bool Sub::auto_loiter_start()
bool ModeAuto::auto_loiter_start()
{
// return failure if GPS is bad
if (!position_ok()) {
if (!sub.position_ok()) {
return false;
}
auto_mode = Auto_Loiter;
sub.auto_mode = Auto_Loiter;
// calculate stopping point
Vector3f stopping_point;
wp_nav.get_wp_stopping_point(stopping_point);
sub.wp_nav.get_wp_stopping_point(stopping_point);
// initialise waypoint controller target to stopping point
wp_nav.set_wp_destination(stopping_point);
sub.wp_nav.set_wp_destination(stopping_point);
// hold yaw at current heading
set_auto_yaw_mode(AUTO_YAW_HOLD);
@ -302,35 +295,35 @@ bool Sub::auto_loiter_start()
// auto_loiter_run - loiter in AUTO flight mode
// called by auto_run at 100hz or more
void Sub::auto_loiter_run()
void ModeAuto::auto_loiter_run()
{
// if not armed set throttle to zero and exit immediately
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control->relax_attitude_controllers();
wp_nav.wp_and_spline_init(); // Reset xy target
sub.wp_nav.wp_and_spline_init(); // Reset xy target
return;
}
// accept pilot input of yaw
float target_yaw_rate = 0;
if (!failsafe.pilot_input) {
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!sub.failsafe.pilot_input) {
target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run waypoint and z-axis position controller
failsafe_terrain_set_status(wp_nav.update_wpnav());
sub.failsafe_terrain_set_status(sub.wp_nav.update_wpnav());
///////////////////////
// update xy outputs //
float lateral_out, forward_out;
translate_wpnav_rp(lateral_out, forward_out);
sub.translate_wpnav_rp(lateral_out, forward_out);
// Send to forward/lateral outputs
motors.set_lateral(lateral_out);
@ -338,102 +331,33 @@ void Sub::auto_loiter_run()
// WP_Nav has set the vertical position control targets
// run the vertical position controller and set output throttle
pos_control.update_z_controller();
position_control->update_z_controller();
// get pilot desired lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max);
// roll & pitch & yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
}
// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
uint8_t Sub::get_default_auto_yaw_mode(bool rtl) const
{
switch (g.wp_yaw_behavior) {
case WP_YAW_BEHAVIOR_NONE:
return AUTO_YAW_HOLD;
break;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
if (rtl) {
return AUTO_YAW_HOLD;
} else {
return AUTO_YAW_LOOK_AT_NEXT_WP;
}
break;
case WP_YAW_BEHAVIOR_LOOK_AHEAD:
return AUTO_YAW_LOOK_AHEAD;
break;
case WP_YAW_BEHAVIOR_CORRECT_XTRACK:
return AUTO_YAW_CORRECT_XTRACK;
break;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP:
default:
return AUTO_YAW_LOOK_AT_NEXT_WP;
break;
}
}
// set_auto_yaw_mode - sets the yaw mode for auto
void Sub::set_auto_yaw_mode(uint8_t yaw_mode)
{
// return immediately if no change
if (auto_yaw_mode == yaw_mode) {
return;
}
auto_yaw_mode = yaw_mode;
// perform initialisation
switch (auto_yaw_mode) {
case AUTO_YAW_LOOK_AT_NEXT_WP:
// wpnav will initialise heading when wpnav's set_destination method is called
break;
case AUTO_YAW_ROI:
// point towards a location held in yaw_look_at_WP
yaw_look_at_WP_bearing = ahrs.yaw_sensor;
break;
case AUTO_YAW_LOOK_AT_HEADING:
// keep heading pointing in the direction held in yaw_look_at_heading
// caller should set the yaw_look_at_heading
break;
case AUTO_YAW_LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
yaw_look_ahead_bearing = ahrs.yaw_sensor;
break;
case AUTO_YAW_RESETTOARMEDYAW:
// initial_armed_bearing will be set during arming so no init required
break;
}
}
// set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode
void Sub::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle)
void ModeAuto::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle)
{
// get current yaw target
int32_t curr_yaw_target = attitude_control.get_att_target_euler_cd().z;
int32_t curr_yaw_target = attitude_control->get_att_target_euler_cd().z;
// get final angle, 1 = Relative, 0 = Absolute
if (relative_angle == 0) {
// absolute angle
yaw_look_at_heading = wrap_360_cd(angle_deg * 100);
sub.yaw_look_at_heading = wrap_360_cd(angle_deg * 100);
} else {
// relative angle
if (direction < 0) {
angle_deg = -angle_deg;
}
yaw_look_at_heading = wrap_360_cd((angle_deg*100+curr_yaw_target));
sub.yaw_look_at_heading = wrap_360_cd((angle_deg*100+curr_yaw_target));
}
// get turn speed
@ -441,10 +365,10 @@ void Sub::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int
// see AP_Float _slew_yaw in AC_AttitudeControl
if (is_zero(turn_rate_dps)) {
// default to regular auto slew rate
yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE;
sub.yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE;
} else {
int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps;
yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec
int32_t turn_rate = (wrap_180_cd(sub.yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps;
sub.yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec
}
// set yaw mode
@ -454,7 +378,7 @@ void Sub::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int
}
// set_auto_yaw_roi - sets the yaw to look at roi for auto mode
void Sub::set_auto_yaw_roi(const Location &roi_location)
void ModeAuto::set_auto_yaw_roi(const Location &roi_location)
{
// if location is zero lat, lon and altitude turn off ROI
if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) {
@ -462,20 +386,20 @@ void Sub::set_auto_yaw_roi(const Location &roi_location)
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
#if HAL_MOUNT_ENABLED
// switch off the camera tracking if enabled
if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
camera_mount.set_mode_to_default();
if (sub.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
sub.camera_mount.set_mode_to_default();
}
#endif // HAL_MOUNT_ENABLED
} else {
#if HAL_MOUNT_ENABLED
// check if mount type requires us to rotate the sub
if (!camera_mount.has_pan_control()) {
if (roi_location.get_vector_from_origin_NEU(roi_WP)) {
if (!sub.camera_mount.has_pan_control()) {
if (roi_location.get_vector_from_origin_NEU(sub.roi_WP)) {
set_auto_yaw_mode(AUTO_YAW_ROI);
}
}
// send the command to the camera mount
camera_mount.set_roi_target(roi_location);
sub.camera_mount.set_roi_target(roi_location);
// TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below)
// 0: do nothing
@ -485,70 +409,18 @@ void Sub::set_auto_yaw_roi(const Location &roi_location)
// 4: point at a target given a target id (can't be implemented)
#else
// if we have no camera mount aim the sub at the location
if (roi_location.get_vector_from_origin_NEU(roi_WP)) {
if (roi_location.get_vector_from_origin_NEU(sub.roi_WP)) {
set_auto_yaw_mode(AUTO_YAW_ROI);
}
#endif // HAL_MOUNT_ENABLED
}
}
// get_auto_heading - returns target heading depending upon auto_yaw_mode
// 100hz update rate
float Sub::get_auto_heading()
{
switch (auto_yaw_mode) {
case AUTO_YAW_ROI:
// point towards a location held in roi_WP
return get_roi_yaw();
break;
case AUTO_YAW_LOOK_AT_HEADING:
// keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed
return yaw_look_at_heading;
break;
case AUTO_YAW_LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
return get_look_ahead_yaw();
break;
case AUTO_YAW_RESETTOARMEDYAW:
// changes yaw to be same as when quad was armed
return initial_armed_bearing;
break;
case AUTO_YAW_CORRECT_XTRACK: {
// TODO return current yaw if not in appropriate mode
// Bearing of current track (centidegrees)
float track_bearing = get_bearing_cd(wp_nav.get_wp_origin().xy(), wp_nav.get_wp_destination().xy());
// Bearing from current position towards intermediate position target (centidegrees)
const Vector2f target_vel_xy{pos_control.get_vel_target_cms().x, pos_control.get_vel_target_cms().y};
float angle_error = 0.0f;
if (target_vel_xy.length() >= pos_control.get_max_speed_xy_cms() * 0.1f) {
const float desired_angle_cd = degrees(target_vel_xy.angle()) * 100.0f;
angle_error = wrap_180_cd(desired_angle_cd - track_bearing);
}
float angle_limited = constrain_float(angle_error, -g.xtrack_angle_limit * 100.0f, g.xtrack_angle_limit * 100.0f);
return wrap_360_cd(track_bearing + angle_limited);
}
break;
case AUTO_YAW_LOOK_AT_NEXT_WP:
default:
// point towards next waypoint.
// we don't use wp_bearing because we don't want the vehicle to turn too much during flight
return wp_nav.get_yaw();
break;
}
}
// Return true if it is possible to recover from a rangefinder failure
bool Sub::auto_terrain_recover_start()
bool ModeAuto::auto_terrain_recover_start()
{
// Check rangefinder status to see if recovery is possible
switch (rangefinder.status_orient(ROTATION_PITCH_270)) {
switch (sub.rangefinder.status_orient(ROTATION_PITCH_270)) {
case RangeFinder::Status::OutOfRangeLow:
case RangeFinder::Status::OutOfRangeHigh:
@ -556,7 +428,7 @@ bool Sub::auto_terrain_recover_start()
// RangeFinder::Good if just one valid sample was obtained recently, but ::rangefinder_state.alt_healthy
// requires several consecutive valid readings for wpnav to accept rangefinder data
case RangeFinder::Status::Good:
auto_mode = Auto_TerrainRecover;
sub.auto_mode = Auto_TerrainRecover;
break;
// Not connected or no data
@ -565,21 +437,21 @@ bool Sub::auto_terrain_recover_start()
}
// Initialize recovery timeout time
fs_terrain_recover_start_ms = AP_HAL::millis();
sub.fs_terrain_recover_start_ms = AP_HAL::millis();
// Stop mission
mission.stop();
sub.mission.stop();
// Reset xy target
loiter_nav.clear_pilot_desired_acceleration();
loiter_nav.init_target();
sub.loiter_nav.clear_pilot_desired_acceleration();
sub.loiter_nav.init_target();
// Reset z axis controller
pos_control.relax_z_controller(motors.get_throttle_hover());
position_control->relax_z_controller(motors.get_throttle_hover());
// initialize vertical maximum speeds and acceleration
pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z());
pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z());
position_control->set_max_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z());
position_control->set_correction_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z());
gcs().send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery");
return true;
@ -588,7 +460,7 @@ bool Sub::auto_terrain_recover_start()
// Attempt recovery from terrain failsafe
// If recovery is successful resume mission
// If recovery fails revert to failsafe action
void Sub::auto_terrain_recover_run()
void ModeAuto::auto_terrain_recover_run()
{
float target_climb_rate = 0;
static uint32_t rangefinder_recovery_ms = 0;
@ -596,23 +468,23 @@ void Sub::auto_terrain_recover_run()
// if not armed set throttle to zero and exit immediately
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control->relax_attitude_controllers();
loiter_nav.init_target(); // Reset xy target
pos_control.relax_z_controller(motors.get_throttle_hover()); // Reset z axis controller
sub.loiter_nav.init_target(); // Reset xy target
position_control->relax_z_controller(motors.get_throttle_hover()); // Reset z axis controller
return;
}
switch (rangefinder.status_orient(ROTATION_PITCH_270)) {
switch (sub.rangefinder.status_orient(ROTATION_PITCH_270)) {
case RangeFinder::Status::OutOfRangeLow:
target_climb_rate = wp_nav.get_default_speed_up();
target_climb_rate = sub.wp_nav.get_default_speed_up();
rangefinder_recovery_ms = 0;
break;
case RangeFinder::Status::OutOfRangeHigh:
target_climb_rate = wp_nav.get_default_speed_down();
target_climb_rate = sub.wp_nav.get_default_speed_down();
rangefinder_recovery_ms = 0;
break;
@ -620,21 +492,21 @@ void Sub::auto_terrain_recover_run()
target_climb_rate = 0; // Attempt to hold current depth
if (rangefinder_state.alt_healthy) {
if (sub.rangefinder_state.alt_healthy) {
// Start timer as soon as rangefinder is healthy
if (rangefinder_recovery_ms == 0) {
rangefinder_recovery_ms = AP_HAL::millis();
pos_control.relax_z_controller(motors.get_throttle_hover()); // Reset alt hold targets
position_control->relax_z_controller(motors.get_throttle_hover()); // Reset alt hold targets
}
// 1.5 seconds of healthy rangefinder means we can resume mission with terrain enabled
if (AP_HAL::millis() > rangefinder_recovery_ms + 1500) {
gcs().send_text(MAV_SEVERITY_INFO, "Terrain failsafe recovery successful!");
failsafe_terrain_set_status(true); // Reset failsafe timers
failsafe.terrain = false; // Clear flag
auto_mode = Auto_Loiter; // Switch back to loiter for next iteration
mission.resume(); // Resume mission
sub.failsafe_terrain_set_status(true); // Reset failsafe timers
sub.failsafe.terrain = false; // Clear flag
sub.auto_mode = Auto_Loiter; // Switch back to loiter for next iteration
sub.mission.resume(); // Resume mission
rangefinder_recovery_ms = 0; // Reset for subsequent recoveries
}
@ -646,25 +518,25 @@ void Sub::auto_terrain_recover_run()
// Terrain failsafe recovery has failed, terrain data is not available
// and rangefinder is not connected, or has stopped responding
gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery failure: No Rangefinder!");
failsafe_terrain_act();
sub.failsafe_terrain_act();
rangefinder_recovery_ms = 0;
return;
}
// exit on failure (timeout)
if (AP_HAL::millis() > fs_terrain_recover_start_ms + FS_TERRAIN_RECOVER_TIMEOUT_MS) {
if (AP_HAL::millis() > sub.fs_terrain_recover_start_ms + FS_TERRAIN_RECOVER_TIMEOUT_MS) {
// Recovery has failed, revert to failsafe action
gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery timeout!");
failsafe_terrain_act();
sub.failsafe_terrain_act();
}
// run loiter controller
loiter_nav.update();
sub.loiter_nav.update();
///////////////////////
// update xy targets //
float lateral_out, forward_out;
translate_wpnav_rp(lateral_out, forward_out);
sub.translate_wpnav_rp(lateral_out, forward_out);
// Send to forward/lateral outputs
motors.set_lateral(lateral_out);
@ -672,8 +544,8 @@ void Sub::auto_terrain_recover_run()
/////////////////////
// update z target //
pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate);
pos_control.update_z_controller();
position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
position_control->update_z_controller();
////////////////////////////
// update angular targets //
@ -681,11 +553,11 @@ void Sub::auto_terrain_recover_run()
float target_pitch = 0;
// convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
// To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats
sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max);
float target_yaw_rate = 0;
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
}

48
ArduSub/control_circle.cpp
View File

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

331
ArduSub/control_guided.cpp
View File

@ -29,9 +29,9 @@ struct Guided_Limit {
} guided_limit;
// guided_init - initialise guided controller
bool Sub::guided_init(bool ignore_checks)
bool ModeGuided::init(bool ignore_checks)
{
if (!position_ok() && !ignore_checks) {
if (!sub.position_ok() && !ignore_checks) {
return false;
}
@ -40,73 +40,107 @@ bool Sub::guided_init(bool ignore_checks)
return true;
}
// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
autopilot_yaw_mode ModeGuided::get_default_auto_yaw_mode(bool rtl) const
{
switch (g.wp_yaw_behavior) {
case WP_YAW_BEHAVIOR_NONE:
return AUTO_YAW_HOLD;
break;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
if (rtl) {
return AUTO_YAW_HOLD;
} else {
return AUTO_YAW_LOOK_AT_NEXT_WP;
}
break;
case WP_YAW_BEHAVIOR_LOOK_AHEAD:
return AUTO_YAW_LOOK_AHEAD;
break;
case WP_YAW_BEHAVIOR_CORRECT_XTRACK:
return AUTO_YAW_CORRECT_XTRACK;
break;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP:
default:
return AUTO_YAW_LOOK_AT_NEXT_WP;
break;
}
}
// initialise guided mode's position controller
void Sub::guided_pos_control_start()
void ModeGuided::guided_pos_control_start()
{
// set to position control mode
guided_mode = Guided_WP;
sub.guided_mode = Guided_WP;
// initialise waypoint controller
wp_nav.wp_and_spline_init();
sub.wp_nav.wp_and_spline_init();
// initialise wpnav to stopping point at current altitude
// To-Do: set to current location if disarmed?
// To-Do: set to stopping point altitude?
Vector3f stopping_point;
wp_nav.get_wp_stopping_point(stopping_point);
sub.wp_nav.get_wp_stopping_point(stopping_point);
// no need to check return status because terrain data is not used
wp_nav.set_wp_destination(stopping_point, false);
sub.wp_nav.set_wp_destination(stopping_point, false);
// initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
}
// initialise guided mode's velocity controller
void Sub::guided_vel_control_start()
void ModeGuided::guided_vel_control_start()
{
// set guided_mode to velocity controller
guided_mode = Guided_Velocity;
sub.guided_mode = Guided_Velocity;
// initialize vertical maximum speeds and acceleration
pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
// initialise velocity controller
pos_control.init_z_controller();
pos_control.init_xy_controller();
position_control->init_z_controller();
position_control->init_xy_controller();
}
// initialise guided mode's posvel controller
void Sub::guided_posvel_control_start()
void ModeGuided::guided_posvel_control_start()
{
// set guided_mode to velocity controller
guided_mode = Guided_PosVel;
sub.guided_mode = Guided_PosVel;
// set vertical speed and acceleration
pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z());
pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z());
position_control->set_max_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z());
position_control->set_correction_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z());
// initialise velocity controller
pos_control.init_z_controller();
pos_control.init_xy_controller();
position_control->init_z_controller();
position_control->init_xy_controller();
// pilot always controls yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
// initialise guided mode's angle controller
void Sub::guided_angle_control_start()
void ModeGuided::guided_angle_control_start()
{
// set guided_mode to velocity controller
guided_mode = Guided_Angle;
sub.guided_mode = Guided_Angle;
// set vertical speed and acceleration
pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z());
pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z());
position_control->set_max_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z());
position_control->set_correction_speed_accel_z(sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up(), sub.wp_nav.get_accel_z());
// initialise velocity controller
pos_control.init_z_controller();
position_control->init_z_controller();
// initialise targets
guided_angle_state.update_time_ms = AP_HAL::millis();
@ -122,17 +156,17 @@ void Sub::guided_angle_control_start()
// guided_set_destination - sets guided mode's target destination
// Returns true if the fence is enabled and guided waypoint is within the fence
// else return false if the waypoint is outside the fence
bool Sub::guided_set_destination(const Vector3f& destination)
bool ModeGuided::guided_set_destination(const Vector3f& destination)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
if (sub.guided_mode != Guided_WP) {
guided_pos_control_start();
}
#if AP_FENCE_ENABLED
// reject destination if outside the fence
const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
if (!fence.check_destination_within_fence(dest_loc)) {
if (!sub.fence.check_destination_within_fence(dest_loc)) {
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
@ -140,34 +174,34 @@ bool Sub::guided_set_destination(const Vector3f& destination)
#endif
// no need to check return status because terrain data is not used
wp_nav.set_wp_destination(destination, false);
sub.wp_nav.set_wp_destination(destination, false);
// log target
Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
sub.Log_Write_GuidedTarget(sub.guided_mode, destination, Vector3f());
return true;
}
// sets guided mode's target from a Location object
// returns false if destination could not be set (probably caused by missing terrain data)
// or if the fence is enabled and guided waypoint is outside the fence
bool Sub::guided_set_destination(const Location& dest_loc)
bool ModeGuided::guided_set_destination(const Location& dest_loc)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
if (sub.guided_mode != Guided_WP) {
guided_pos_control_start();
}
#if AP_FENCE_ENABLED
// reject destination outside the fence.
// Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
if (!fence.check_destination_within_fence(dest_loc)) {
if (!sub.fence.check_destination_within_fence(dest_loc)) {
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
#endif
if (!wp_nav.set_wp_destination_loc(dest_loc)) {
if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) {
// failure to set destination can only be because of missing terrain data
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
// failure is propagated to GCS with NAK
@ -175,36 +209,36 @@ bool Sub::guided_set_destination(const Location& dest_loc)
}
// log target
Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
sub.Log_Write_GuidedTarget(sub.guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
return true;
}
// guided_set_velocity - sets guided mode's target velocity
void Sub::guided_set_velocity(const Vector3f& velocity)
void ModeGuided::guided_set_velocity(const Vector3f& velocity)
{
// check we are in velocity control mode
if (guided_mode != Guided_Velocity) {
if (sub.guided_mode != Guided_Velocity) {
guided_vel_control_start();
}
update_time_ms = AP_HAL::millis();
// set position controller velocity target
pos_control.set_vel_desired_cms(velocity);
position_control->set_vel_desired_cms(velocity);
}
// set guided mode posvel target
bool Sub::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity)
bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity)
{
// check we are in velocity control mode
if (guided_mode != Guided_PosVel) {
if (sub.guided_mode != Guided_PosVel) {
guided_posvel_control_start();
}
#if AP_FENCE_ENABLED
// reject destination if outside the fence
const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
if (!fence.check_destination_within_fence(dest_loc)) {
if (!sub.fence.check_destination_within_fence(dest_loc)) {
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
@ -215,21 +249,21 @@ bool Sub::guided_set_destination_posvel(const Vector3f& destination, const Vecto
posvel_pos_target_cm = destination.topostype();
posvel_vel_target_cms = velocity;
pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f());
position_control->input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f());
float dz = posvel_pos_target_cm.z;
pos_control.input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0);
position_control->input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0);
posvel_pos_target_cm.z = dz;
// log target
Log_Write_GuidedTarget(guided_mode, destination, velocity);
sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity);
return true;
}
// set guided mode angle target
void Sub::guided_set_angle(const Quaternion &q, float climb_rate_cms)
void ModeGuided::guided_set_angle(const Quaternion &q, float climb_rate_cms)
{
// check we are in velocity control mode
if (guided_mode != Guided_Angle) {
if (sub.guided_mode != Guided_Angle) {
guided_angle_control_start();
}
@ -245,10 +279,10 @@ void Sub::guided_set_angle(const Quaternion &q, float climb_rate_cms)
// guided_run - runs the guided controller
// should be called at 100hz or more
void Sub::guided_run()
void ModeGuided::run()
{
// call the correct auto controller
switch (guided_mode) {
switch (sub.guided_mode) {
case Guided_WP:
// run position controller
@ -274,23 +308,23 @@ void Sub::guided_run()
// guided_pos_control_run - runs the guided position controller
// called from guided_run
void Sub::guided_pos_control_run()
void ModeGuided::guided_pos_control_run()
{
// if motors not enabled set throttle to zero and exit immediately
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
wp_nav.wp_and_spline_init();
attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control->relax_attitude_controllers();
sub.wp_nav.wp_and_spline_init();
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.pilot_input) {
if (!sub.failsafe.pilot_input) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
@ -300,10 +334,10 @@ void Sub::guided_pos_control_run()
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run waypoint controller
failsafe_terrain_set_status(wp_nav.update_wpnav());
sub.failsafe_terrain_set_status(sub.wp_nav.update_wpnav());
float lateral_out, forward_out;
translate_wpnav_rp(lateral_out, forward_out);
sub.translate_wpnav_rp(lateral_out, forward_out);
// Send to forward/lateral outputs
motors.set_lateral(lateral_out);
@ -311,39 +345,39 @@ void Sub::guided_pos_control_run()
// WP_Nav has set the vertical position control targets
// run the vertical position controller and set output throttle
pos_control.update_z_controller();
position_control->update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
if (sub.auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch & yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
} else {
// roll, pitch from pilot, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
}
}
// guided_vel_control_run - runs the guided velocity controller
// called from guided_run
void Sub::guided_vel_control_run()
void ModeGuided::guided_vel_control_run()
{
// ifmotors not enabled set throttle to zero and exit immediately
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control->relax_attitude_controllers();
// initialise velocity controller
pos_control.init_z_controller();
pos_control.init_xy_controller();
position_control->init_z_controller();
position_control->init_xy_controller();
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.pilot_input) {
if (!sub.failsafe.pilot_input) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
@ -354,54 +388,54 @@ void Sub::guided_vel_control_run()
// set velocity to zero if no updates received for 3 seconds
uint32_t tnow = AP_HAL::millis();
if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_vel_desired_cms().is_zero()) {
pos_control.set_vel_desired_cms(Vector3f(0,0,0));
if (tnow - update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !position_control->get_vel_desired_cms().is_zero()) {
position_control->set_vel_desired_cms(Vector3f(0,0,0));
}
pos_control.stop_pos_xy_stabilisation();
position_control->stop_pos_xy_stabilisation();
// call velocity controller which includes z axis controller
pos_control.update_xy_controller();
pos_control.update_z_controller();
position_control->update_xy_controller();
position_control->update_z_controller();
float lateral_out, forward_out;
translate_pos_control_rp(lateral_out, forward_out);
sub.translate_pos_control_rp(lateral_out, forward_out);
// Send to forward/lateral outputs
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
if (sub.auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch & yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
} else {
// roll, pitch from pilot, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
}
}
// guided_posvel_control_run - runs the guided posvel controller
// called from guided_run
void Sub::guided_posvel_control_run()
void ModeGuided::guided_posvel_control_run()
{
// if motors not enabled set throttle to zero and exit immediately
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control->relax_attitude_controllers();
// initialise velocity controller
pos_control.init_z_controller();
pos_control.init_xy_controller();
position_control->init_z_controller();
position_control->init_xy_controller();
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.pilot_input) {
if (!sub.failsafe.pilot_input) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
@ -417,47 +451,47 @@ void Sub::guided_posvel_control_run()
}
// advance position target using velocity target
posvel_pos_target_cm += (posvel_vel_target_cms * pos_control.get_dt()).topostype();
posvel_pos_target_cm += (posvel_vel_target_cms * position_control->get_dt()).topostype();
// send position and velocity targets to position controller
pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f());
position_control->input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f());
float pz = posvel_pos_target_cm.z;
pos_control.input_pos_vel_accel_z(pz, posvel_vel_target_cms.z, 0);
position_control->input_pos_vel_accel_z(pz, posvel_vel_target_cms.z, 0);
posvel_pos_target_cm.z = pz;
// run position controller
pos_control.update_xy_controller();
pos_control.update_z_controller();
position_control->update_xy_controller();
position_control->update_z_controller();
float lateral_out, forward_out;
translate_pos_control_rp(lateral_out, forward_out);
sub.translate_pos_control_rp(lateral_out, forward_out);
// Send to forward/lateral outputs
motors.set_lateral(lateral_out);
motors.set_forward(forward_out);
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
if (sub.auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch & yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
} else {
// roll, pitch from pilot, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
attitude_control->input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
}
}
// guided_angle_control_run - runs the guided angle controller
// called from guided_run
void Sub::guided_angle_control_run()
void ModeGuided::guided_angle_control_run()
{
// if motors not enabled set throttle to zero and exit immediately
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0.0f,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
attitude_control->set_throttle_out(0.0f,true,g.throttle_filt);
attitude_control->relax_attitude_controllers();
// initialise velocity controller
pos_control.init_z_controller();
position_control->init_z_controller();
return;
}
@ -465,7 +499,7 @@ void Sub::guided_angle_control_run()
float roll_in = guided_angle_state.roll_cd;
float pitch_in = guided_angle_state.pitch_cd;
float total_in = norm(roll_in, pitch_in);
float angle_max = MIN(attitude_control.get_althold_lean_angle_max_cd(), aparm.angle_max);
float angle_max = MIN(attitude_control->get_althold_lean_angle_max_cd(), sub.aparm.angle_max);
if (total_in > angle_max) {
float ratio = angle_max / total_in;
roll_in *= ratio;
@ -476,7 +510,7 @@ void Sub::guided_angle_control_run()
float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);
// constrain climb rate
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up());
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -sub.wp_nav.get_default_speed_down(), sub.wp_nav.get_default_speed_up());
// check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
uint32_t tnow = AP_HAL::millis();
@ -490,17 +524,17 @@ void Sub::guided_angle_control_run()
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);
attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);
// call position controller
pos_control.set_pos_target_z_from_climb_rate_cm(climb_rate_cms);
pos_control.update_z_controller();
position_control->set_pos_target_z_from_climb_rate_cm(climb_rate_cms);
position_control->update_z_controller();
}
// Guided Limit code
// guided_limit_clear - clear/turn off guided limits
void Sub::guided_limit_clear()
void ModeGuided::guided_limit_clear()
{
guided_limit.timeout_ms = 0;
guided_limit.alt_min_cm = 0.0f;
@ -508,8 +542,97 @@ void Sub::guided_limit_clear()
guided_limit.horiz_max_cm = 0.0f;
}
// set_auto_yaw_mode - sets the yaw mode for auto
void ModeGuided::set_auto_yaw_mode(autopilot_yaw_mode yaw_mode)
{
// return immediately if no change
if (sub.auto_yaw_mode == yaw_mode) {
return;
}
sub.auto_yaw_mode = yaw_mode;
// perform initialisation
switch (sub.auto_yaw_mode) {
case AUTO_YAW_LOOK_AT_NEXT_WP:
// wpnav will initialise heading when wpnav's set_destination method is called
break;
case AUTO_YAW_ROI:
// point towards a location held in yaw_look_at_WP
sub.yaw_look_at_WP_bearing = ahrs.yaw_sensor;
break;
case AUTO_YAW_LOOK_AT_HEADING:
// keep heading pointing in the direction held in yaw_look_at_heading
// caller should set the yaw_look_at_heading
break;
case AUTO_YAW_LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
sub.yaw_look_ahead_bearing = ahrs.yaw_sensor;
break;
case AUTO_YAW_RESETTOARMEDYAW:
// initial_armed_bearing will be set during arming so no init required
break;
}
}
// get_auto_heading - returns target heading depending upon auto_yaw_mode
// 100hz update rate
float ModeGuided::get_auto_heading()
{
switch (sub.auto_yaw_mode) {
case AUTO_YAW_ROI:
// point towards a location held in roi_WP
return sub.get_roi_yaw();
break;
case AUTO_YAW_LOOK_AT_HEADING:
// keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed
return sub.yaw_look_at_heading;
break;
case AUTO_YAW_LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
return sub.get_look_ahead_yaw();
break;
case AUTO_YAW_RESETTOARMEDYAW:
// changes yaw to be same as when quad was armed
return sub.initial_armed_bearing;
break;
case AUTO_YAW_CORRECT_XTRACK: {
// TODO return current yaw if not in appropriate mode
// Bearing of current track (centidegrees)
float track_bearing = get_bearing_cd(sub.wp_nav.get_wp_origin().xy(), sub.wp_nav.get_wp_destination().xy());
// Bearing from current position towards intermediate position target (centidegrees)
const Vector2f target_vel_xy{position_control->get_vel_target_cms().x, position_control->get_vel_target_cms().y};
float angle_error = 0.0f;
if (target_vel_xy.length() >= position_control->get_max_speed_xy_cms() * 0.1f) {
const float desired_angle_cd = degrees(target_vel_xy.angle()) * 100.0f;
angle_error = wrap_180_cd(desired_angle_cd - track_bearing);
}
float angle_limited = constrain_float(angle_error, -g.xtrack_angle_limit * 100.0f, g.xtrack_angle_limit * 100.0f);
return wrap_360_cd(track_bearing + angle_limited);
}
break;
case AUTO_YAW_LOOK_AT_NEXT_WP:
default:
// point towards next waypoint.
// we don't use wp_bearing because we don't want the vehicle to turn too much during flight
return sub.wp_nav.get_yaw();
break;
}
}
// guided_limit_set - set guided timeout and movement limits
void Sub::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
void ModeGuided::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
{
guided_limit.timeout_ms = timeout_ms;
guided_limit.alt_min_cm = alt_min_cm;
@ -519,7 +642,7 @@ void Sub::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_
// guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking
// only called from AUTO mode's auto_nav_guided_start function
void Sub::guided_limit_init_time_and_pos()
void ModeGuided::guided_limit_init_time_and_pos()
{
// initialise start time
guided_limit.start_time = AP_HAL::millis();
@ -530,7 +653,7 @@ void Sub::guided_limit_init_time_and_pos()
// guided_limit_check - returns true if guided mode has breached a limit
// used when guided is invoked from the NAV_GUIDED_ENABLE mission command
bool Sub::guided_limit_check()
bool ModeGuided::guided_limit_check()
{
// check if we have passed the timeout
if ((guided_limit.timeout_ms > 0) && (AP_HAL::millis() - guided_limit.start_time >= guided_limit.timeout_ms)) {

33
ArduSub/control_manual.cpp
View File

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

8
ArduSub/control_motordetect.cpp
View File

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

64
ArduSub/control_poshold.cpp
View File

@ -7,47 +7,47 @@
#if POSHOLD_ENABLED == ENABLED
// poshold_init - initialise PosHold controller
bool Sub::poshold_init()
bool ModePoshold::init(bool ignore_checks)
{
// fail to initialise PosHold mode if no GPS lock
if (!position_ok()) {
if (!sub.position_ok()) {
return false;
}
// initialize vertical speeds and acceleration
pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration());
pos_control.set_correction_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration());
pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
position_control->set_max_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration());
position_control->set_correction_speed_accel_xy(sub.wp_nav.get_default_speed_xy(), sub.wp_nav.get_wp_acceleration());
position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
// initialise position and desired velocity
pos_control.init_xy_controller_stopping_point();
pos_control.init_z_controller();
position_control->init_xy_controller_stopping_point();
position_control->init_z_controller();
// Stop all thrusters
attitude_control.set_throttle_out(0.5f ,true, g.throttle_filt);
attitude_control.relax_attitude_controllers();
pos_control.relax_z_controller(0.5f);
attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt);
attitude_control->relax_attitude_controllers();
position_control->relax_z_controller(0.5f);
last_pilot_heading = ahrs.yaw_sensor;
sub.last_pilot_heading = ahrs.yaw_sensor;
return true;
}
// poshold_run - runs the PosHold controller
// should be called at 100hz or more
void Sub::poshold_run()
void ModePoshold::run()
{
uint32_t tnow = AP_HAL::millis();
// When unarmed, disable motors and stabilization
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
// Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
attitude_control.set_throttle_out(0.5f ,true, g.throttle_filt);
attitude_control.relax_attitude_controllers();
pos_control.init_xy_controller_stopping_point();
pos_control.relax_z_controller(0.5f);
last_pilot_heading = ahrs.yaw_sensor;
attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt);
attitude_control->relax_attitude_controllers();
position_control->init_xy_controller_stopping_point();
position_control->relax_z_controller(0.5f);
sub.last_pilot_heading = ahrs.yaw_sensor;
return;
}
@ -62,15 +62,15 @@ void Sub::poshold_run()
float lateral_out = 0;
float forward_out = 0;
if (position_ok()) {
if (sub.position_ok()) {
// Allow pilot to reposition the sub
if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) {
pos_control.init_xy_controller_stopping_point();
position_control->init_xy_controller_stopping_point();
}
translate_pos_control_rp(lateral_out, forward_out);
pos_control.update_xy_controller();
sub.translate_pos_control_rp(lateral_out, forward_out);
position_control->update_xy_controller();
} else {
pos_control.init_xy_controller_stopping_point();
position_control->init_xy_controller_stopping_point();
}
motors.set_forward(forward_out + pilot_forward);
motors.set_lateral(lateral_out + pilot_lateral);
@ -78,32 +78,32 @@ void Sub::poshold_run()
// Update attitude //
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
float target_roll, target_pitch;
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max);
// update attitude controller targets
if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
last_pilot_heading = ahrs.yaw_sensor;
last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
sub.last_pilot_heading = ahrs.yaw_sensor;
sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
} else { // hold current heading
// this check is required to prevent bounce back after very fast yaw maneuvers
// the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
if (tnow < sub.last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
target_yaw_rate = 0; // Stop rotation on yaw axis
// call attitude controller with target yaw rate = 0 to decelerate on yaw axis
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
} else { // call attitude controller holding absolute absolute bearing
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true);
attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, sub.last_pilot_heading, true);
}
}

47
ArduSub/control_stabilize.cpp
View File

@ -1,66 +1,65 @@
#include "Sub.h"
// stabilize_init - initialise stabilize controller
bool Sub::stabilize_init()
{
// set target altitude to zero for reporting
pos_control.set_pos_target_z_cm(0);
last_pilot_heading = ahrs.yaw_sensor;
bool ModeStabilize::init(bool ignore_checks) {
// set target altitude to zero for reporting
position_control->set_pos_target_z_cm(0);
sub.last_pilot_heading = ahrs.yaw_sensor;
return true;
return true;
}
// stabilize_run - runs the main stabilize controller
// should be called at 100hz or more
void Sub::stabilize_run()
void ModeStabilize::run()
{
uint32_t tnow = AP_HAL::millis();
uint32_t tnow = AP_HAL::millis();
float target_roll, target_pitch;
// if not armed set throttle to zero and exit immediately
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
last_pilot_heading = ahrs.yaw_sensor;
attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control->relax_attitude_controllers();
sub.last_pilot_heading = ahrs.yaw_sensor;
return;
}
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
// To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats
// TODO2: move into mode.h
sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max);
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// call attitude controller
// update attitude controller targets
if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
last_pilot_heading = ahrs.yaw_sensor;
last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
sub.last_pilot_heading = ahrs.yaw_sensor;
sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
} else { // hold current heading
// this check is required to prevent bounce back after very fast yaw maneuvers
// the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
if (tnow < sub.last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
target_yaw_rate = 0; // Stop rotation on yaw axis
// call attitude controller with target yaw rate = 0 to decelerate on yaw axis
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
} else { // call attitude controller holding absolute absolute bearing
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true);
attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, sub.last_pilot_heading, true);
}
}
// output pilot's throttle
attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);
attitude_control->set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);
//control_in is range -1000-1000
//radio_in is raw pwm value

38
ArduSub/control_surface.cpp
View File

@ -1,24 +1,24 @@
#include "Sub.h"
bool Sub::surface_init()
bool ModeSurface::init(bool ignore_checks)
{
if(!control_check_barometer()) {
if(!sub.control_check_barometer()) {
return false;
}
// initialize vertical speeds and acceleration
pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
position_control->set_max_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
position_control->set_correction_speed_accel_z(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
// initialise position and desired velocity
pos_control.init_z_controller();
position_control->init_z_controller();
return true;
}
void Sub::surface_run()
void ModeSurface::run()
{
float target_roll, target_pitch;
@ -26,36 +26,36 @@ void Sub::surface_run()
if (!motors.armed()) {
motors.output_min();
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
pos_control.init_z_controller();
attitude_control->set_throttle_out(0,true,g.throttle_filt);
attitude_control->relax_attitude_controllers();
position_control->init_z_controller();
return;
}
// Already at surface, hold depth at surface
if (ap.at_surface) {
set_mode(ALT_HOLD, ModeReason::SURFACE_COMPLETE);
if (sub.ap.at_surface) {
set_mode(Mode::Number::ALT_HOLD, ModeReason::SURFACE_COMPLETE);
}
// convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
// To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats
sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max);
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
// set target climb rate
float cmb_rate = constrain_float(fabsf(wp_nav.get_default_speed_up()), 1, pos_control.get_max_speed_up_cms());
float cmb_rate = constrain_float(fabsf(sub.wp_nav.get_default_speed_up()), 1, position_control->get_max_speed_up_cms());
// record desired climb rate for logging
desired_climb_rate = cmb_rate;
sub.desired_climb_rate = cmb_rate;
// update altitude target and call position controller
pos_control.set_pos_target_z_from_climb_rate_cm(cmb_rate);
pos_control.update_z_controller();
position_control->set_pos_target_z_from_climb_rate_cm(cmb_rate);
position_control->update_z_controller();
// pilot has control for repositioning
motors.set_forward(channel_forward->norm_input());

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 Pilot Modes enumeration
enum control_mode_t : uint8_t {
STABILIZE = 0, // manual angle with manual depth/throttle
ACRO = 1, // manual body-frame angular rate with manual depth/throttle
ALT_HOLD = 2, // manual angle with automatic depth/throttle
AUTO = 3, // fully automatic waypoint control using mission commands
GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
CIRCLE = 7, // automatic circular flight with automatic throttle
SURFACE = 9, // automatically return to surface, pilot maintains horizontal control
POSHOLD = 16, // automatic position hold with manual override, with automatic throttle
MANUAL = 19, // Pass-through input with no stabilization
MOTOR_DETECT = 20 // Automatically detect motors orientation
};
// Acro Trainer types
#define ACRO_TRAINER_DISABLED 0
#define ACRO_TRAINER_LEVELING 1
@ -55,32 +41,7 @@ enum control_mode_t : uint8_t {
#define WP_YAW_BEHAVIOR_LOOK_AHEAD 3 // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicotpers)
#define WP_YAW_BEHAVIOR_CORRECT_XTRACK 4 // point towards intermediate position target during line following
// Auto modes
enum AutoMode {
Auto_WP,
Auto_CircleMoveToEdge,
Auto_Circle,
Auto_NavGuided,
Auto_Loiter,
Auto_TerrainRecover
};
// Guided modes
enum GuidedMode {
Guided_WP,
Guided_Velocity,
Guided_PosVel,
Guided_Angle,
};
// RTL states
enum RTLState {
RTL_InitialClimb,
RTL_ReturnHome,
RTL_LoiterAtHome,
RTL_FinalDescent,
RTL_Land
};
// Logging parameters - only 32 messages are available to the vehicle here.
enum LoggingParameters {

24
ArduSub/failsafe.cpp
View File

@ -88,9 +88,9 @@ void Sub::failsafe_sensors_check()
gcs().send_text(MAV_SEVERITY_CRITICAL, "Depth sensor error!");
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_SENSORS, LogErrorCode::BAD_DEPTH);
if (control_mode == ALT_HOLD || control_mode == SURFACE || mode_requires_GPS(control_mode)) {
if (control_mode == Mode::Number::ALT_HOLD || control_mode == Mode::Number::SURFACE || sub.flightmode->requires_GPS()) {
// This should always succeed
if (!set_mode(MANUAL, ModeReason::BAD_DEPTH)) {
if (!set_mode(Mode::Number::MANUAL, ModeReason::BAD_DEPTH)) {
// We should never get here
arming.disarm(AP_Arming::Method::BADFLOWOFCONTROL);
}
@ -156,7 +156,7 @@ void Sub::handle_battery_failsafe(const char* type_str, const int8_t action)
switch((Failsafe_Action)action) {
case Failsafe_Action_Surface:
set_mode(SURFACE, ModeReason::BATTERY_FAILSAFE);
set_mode(Mode::Number::SURFACE, ModeReason::BATTERY_FAILSAFE);
break;
case Failsafe_Action_Disarm:
arming.disarm(AP_Arming::Method::BATTERYFAILSAFE);
@ -299,7 +299,7 @@ void Sub::failsafe_leak_check()
// Handle failsafe action
if (failsafe.leak && g.failsafe_leak == FS_LEAK_SURFACE && motors.armed()) {
set_mode(SURFACE, ModeReason::LEAK_FAILSAFE);
set_mode(Mode::Number::SURFACE, ModeReason::LEAK_FAILSAFE);
}
}
@ -352,11 +352,11 @@ void Sub::failsafe_gcs_check()
if (g.failsafe_gcs == FS_GCS_DISARM) {
arming.disarm(AP_Arming::Method::GCSFAILSAFE);
} else if (g.failsafe_gcs == FS_GCS_HOLD && motors.armed()) {
if (!set_mode(ALT_HOLD, ModeReason::GCS_FAILSAFE)) {
if (!set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_FAILSAFE)) {
arming.disarm(AP_Arming::Method::GCS_FAILSAFE_HOLDFAILED);
}
} else if (g.failsafe_gcs == FS_GCS_SURFACE && motors.armed()) {
if (!set_mode(SURFACE, ModeReason::GCS_FAILSAFE)) {
if (!set_mode(Mode::Number::SURFACE, ModeReason::GCS_FAILSAFE)) {
arming.disarm(AP_Arming::Method::GCS_FAILSAFE_SURFACEFAILED);
}
}
@ -380,7 +380,7 @@ void Sub::failsafe_crash_check()
}
// return immediately if we are not in an angle stabilized flight mode
if (control_mode == ACRO || control_mode == MANUAL) {
if (control_mode == Mode::Number::ACRO || control_mode == Mode::Number::MANUAL) {
last_crash_check_pass_ms = tnow;
failsafe.crash = false;
return;
@ -425,7 +425,7 @@ void Sub::failsafe_crash_check()
void Sub::failsafe_terrain_check()
{
// trigger with 5 seconds of failures while in AUTO mode
bool valid_mode = (control_mode == AUTO || control_mode == GUIDED);
bool valid_mode = (control_mode == Mode::Number::AUTO || control_mode == Mode::Number::GUIDED);
bool timeout = (failsafe.terrain_last_failure_ms - failsafe.terrain_first_failure_ms) > FS_TERRAIN_TIMEOUT_MS;
bool trigger_event = valid_mode && timeout;
@ -470,7 +470,7 @@ void Sub::failsafe_terrain_on_event()
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED);
// If rangefinder is enabled, we can recover from this failsafe
if (!rangefinder_state.enabled || !auto_terrain_recover_start()) {
if (!rangefinder_state.enabled || !sub.mode_auto.auto_terrain_recover_start()) {
failsafe_terrain_act();
}
@ -482,14 +482,14 @@ void Sub::failsafe_terrain_act()
{
switch (g.failsafe_terrain) {
case FS_TERRAIN_HOLD:
if (!set_mode(POSHOLD, ModeReason::TERRAIN_FAILSAFE)) {
set_mode(ALT_HOLD, ModeReason::TERRAIN_FAILSAFE);
if (!set_mode(Mode::Number::POSHOLD, ModeReason::TERRAIN_FAILSAFE)) {
set_mode(Mode::Number::ALT_HOLD, ModeReason::TERRAIN_FAILSAFE);
}
AP_Notify::events.failsafe_mode_change = 1;
break;
case FS_TERRAIN_SURFACE:
set_mode(SURFACE, ModeReason::TERRAIN_FAILSAFE);
set_mode(Mode::Number::SURFACE, ModeReason::TERRAIN_FAILSAFE);
AP_Notify::events.failsafe_mode_change = 1;
break;

2
ArduSub/fence.cpp
View File

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

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

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()
{
// Motor detection mode controls the thrusters directly
if (control_mode == MOTOR_DETECT){
if (control_mode == Mode::Number::MOTOR_DETECT){
return;
}
// check if we are performing the motor test