From 07f4603533439088d3ac5aba48750fb5e6498cda Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 19 Jul 2017 11:19:08 +0900 Subject: [PATCH] Rover: integrate mode class --- APMrover2/APMrover2.cpp | 160 +-------------------------- APMrover2/GCS_Mavlink.cpp | 73 +++++------- APMrover2/Log.cpp | 6 +- APMrover2/Rover.cpp | 2 +- APMrover2/Rover.h | 47 ++++---- APMrover2/Steering.cpp | 196 +-------------------------------- APMrover2/commands.cpp | 5 +- APMrover2/commands_logic.cpp | 24 ++-- APMrover2/commands_process.cpp | 2 +- APMrover2/control_modes.cpp | 56 +++++++--- APMrover2/crash_check.cpp | 4 +- APMrover2/defines.h | 6 - APMrover2/failsafe.cpp | 8 +- APMrover2/radio.cpp | 2 +- APMrover2/sensors.cpp | 30 ++--- APMrover2/system.cpp | 95 +++++----------- 16 files changed, 160 insertions(+), 556 deletions(-) diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 2422f596db..e8c1adf3f7 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -277,7 +277,7 @@ void Rover::update_logging1(void) void Rover::update_logging2(void) { if (should_log(MASK_LOG_STEERING)) { - if (control_mode == STEERING || control_mode == AUTO || control_mode == RTL || control_mode == GUIDED) { + if (!control_mode->manual_steering()) { Log_Write_Steering(); } } @@ -413,166 +413,12 @@ void Rover::update_GPS_10Hz(void) void Rover::update_current_mode(void) { - switch (control_mode) { - case AUTO: - case RTL: - if (!in_auto_reverse) { - set_reverse(false); - } - if (!do_auto_rotation) { - calc_lateral_acceleration(); - calc_nav_steer(); - calc_throttle(g.speed_cruise); - } else { - do_yaw_rotation(); - } - break; - - case GUIDED: { - if (!in_auto_reverse) { - set_reverse(false); - } - switch (guided_mode) { - case Guided_Angle: - nav_set_yaw_speed(); - break; - - case Guided_WP: - if (rtl_complete || verify_RTL()) { - // we have reached destination so stop where we are - if (fabsf(g2.motors.get_throttle()) > g.throttle_min.get()) { - gcs().send_mission_item_reached_message(0); - } - g2.motors.set_throttle(g.throttle_min.get()); - g2.motors.set_steering(0.0f); - lateral_acceleration = 0.0f; - } else { - calc_lateral_acceleration(); - calc_nav_steer(); - calc_throttle(rover.guided_control.target_speed); - Log_Write_GuidedTarget(guided_mode, Vector3f(next_WP.lat, next_WP.lng, next_WP.alt), - Vector3f(rover.guided_control.target_speed, g2.motors.get_throttle(), 0.0f)); - } - break; - - case Guided_Velocity: - nav_set_speed(); - break; - - default: - gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode"); - break; - } - break; - } - - case STEERING: { - /* - in steering mode we control lateral acceleration - directly. We first calculate the maximum lateral - acceleration at full steering lock for this speed. That is - V^2/R where R is the radius of turn. We get the radius of - turn from half the STEER2SRV_P. - */ - float max_g_force = ground_speed * ground_speed / steerController.get_turn_radius(); - - // constrain to user set TURN_MAX_G - max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS); - - lateral_acceleration = max_g_force * (channel_steer->get_control_in()/4500.0f); - calc_nav_steer(); - - // and throttle gives speed in proportion to cruise speed, up - // to 50% throttle, then uses nudging above that. - float target_speed = channel_throttle->get_control_in() * 0.01f * 2 * g.speed_cruise; - set_reverse(target_speed < 0); - if (in_reverse) { - target_speed = constrain_float(target_speed, -g.speed_cruise, 0); - } else { - target_speed = constrain_float(target_speed, 0, g.speed_cruise); - } - calc_throttle(target_speed); - break; - } - - case LEARNING: - case MANUAL: - // mark us as in_reverse when using a negative throttle to - // stop AHRS getting off - set_reverse(is_negative(g2.motors.get_throttle())); - break; - - case HOLD: - // hold position - stop motors and center steering - g2.motors.set_throttle(0.0f); - g2.motors.set_steering(0.0f); - if (!in_auto_reverse) { - set_reverse(false); - } - break; - - case INITIALISING: - break; - } + control_mode->update(); } void Rover::update_navigation() { - switch (control_mode) { - case MANUAL: - case HOLD: - case LEARNING: - case STEERING: - case INITIALISING: - break; - - case AUTO: - mission.update(); - if (do_auto_rotation) { - do_yaw_rotation(); - } - break; - - case RTL: - // no loitering around the wp with the rover, goes direct to the wp position - if (verify_RTL()) { - g2.motors.set_throttle(g.throttle_min.get()); - set_mode(HOLD); - } else { - calc_lateral_acceleration(); - calc_nav_steer(); - } - break; - - case GUIDED: - switch (guided_mode) { - case Guided_Angle: - nav_set_yaw_speed(); - break; - - case Guided_WP: - // no loitering around the wp with the rover, goes direct to the wp position - if (rtl_complete || verify_RTL()) { - // we have reached destination so stop where we are - g2.motors.set_throttle(g.throttle_min.get()); - g2.motors.set_steering(0.0f); - lateral_acceleration = 0.0f; - } else { - calc_lateral_acceleration(); - calc_nav_steer(); - } - break; - - case Guided_Velocity: - nav_set_speed(); - break; - - default: - gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode"); - break; - } - break; - } + control_mode->update_navigation(); } AP_HAL_MAIN_CALLBACKS(&rover); diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index ede0f47c7f..9726fbbb21 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -7,7 +7,6 @@ void Rover::send_heartbeat(mavlink_channel_t chan) { uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; uint8_t system_status = MAV_STATE_ACTIVE; - const uint32_t custom_mode = control_mode; if (failsafe.triggered != 0) { system_status = MAV_STATE_CRITICAL; @@ -21,30 +20,16 @@ void Rover::send_heartbeat(mavlink_channel_t chan) // only get useful information from the custom_mode, which maps to // the APM flight mode and has a well defined meaning in the // ArduPlane documentation - switch (control_mode) { - case MANUAL: - case LEARNING: - case STEERING: - base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - break; - case AUTO: - case RTL: - case GUIDED: - 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 - // positions", which APM does not currently do - break; - case INITIALISING: - system_status = MAV_STATE_CALIBRATING; - break; - case HOLD: - system_status = 0; - break; + if (control_mode->has_manual_input()) { + base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } -#if defined(ENABLE_STICK_MIXING) && (ENABLE_STICK_MIXING == ENABLED) - if (control_mode != INITIALISING) { + if (control_mode->is_autopilot_mode()) { + base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + } + +#if defined(ENABLE_STICK_MIXING) && (ENABLE_STICK_MIXING == ENABLED) // TODO ???? Remove ! + if (control_mode->stick_mixing_enabled()) { // all modes except INITIALISING have some form of manual // override if stick mixing is enabled base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; @@ -54,9 +39,14 @@ void Rover::send_heartbeat(mavlink_channel_t chan) #if HIL_MODE != HIL_MODE_DISABLED base_mode |= MAV_MODE_FLAG_HIL_ENABLED; #endif - + if (control_mode == &mode_initializing) { + system_status = MAV_STATE_CALIBRATING; + } + if (control_mode == &mode_hold) { + system_status = MAV_STATE_STANDBY; + } // we are armed if we are not initialising - if (control_mode != INITIALISING && arming.is_armed()) { + if (control_mode != &mode_initializing && arming.is_armed()) { base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -65,7 +55,7 @@ void Rover::send_heartbeat(mavlink_channel_t chan) gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(MAV_TYPE_GROUND_ROVER, base_mode, - custom_mode, + control_mode->mode_number(), system_status); } @@ -140,7 +130,7 @@ void Rover::send_nav_controller_output(mavlink_channel_t chan) { mavlink_msg_nav_controller_output_send( chan, - lateral_acceleration, // use nav_roll to hold demanded Y accel + control_mode->lateral_acceleration, // use nav_roll to hold demanded Y accel ahrs.groundspeed() * ins.get_gyro().z, // use nav_pitch to hold actual Y accel nav_controller->nav_bearing_cd() * 0.01f, nav_controller->target_bearing_cd() * 0.01f, @@ -343,7 +333,7 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id) break; case MSG_NAV_CONTROLLER_OUTPUT: - if (rover.control_mode != MANUAL) { + if (rover.control_mode->is_autopilot_mode()) { CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); rover.send_nav_controller_output(chan); } @@ -672,7 +662,7 @@ GCS_MAVLINK_Rover::data_stream_send(void) if (stream_trigger(STREAM_EXTRA1)) { send_message(MSG_ATTITUDE); send_message(MSG_SIMSTATE); - if (rover.control_mode != MANUAL) { + if (rover.control_mode->is_autopilot_mode()) { send_message(MSG_PID_TUNING); } } @@ -708,14 +698,11 @@ GCS_MAVLINK_Rover::data_stream_send(void) bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd) { - if (rover.control_mode != GUIDED) { + if (rover.control_mode != &rover.mode_guided) { // only accept position updates when in GUIDED mode return false; } - // This method is only called when we are in Guided WP GUIDED mode - rover.guided_mode = Guided_WP; - // make any new wp uploaded instant (in case we are already in Guided mode) rover.set_guided_WP(cmd.content.location); return true; @@ -796,7 +783,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) switch (packet.command) { case MAV_CMD_NAV_RETURN_TO_LAUNCH: - rover.set_mode(RTL); + rover.set_mode(rover.mode_rtl); result = MAV_RESULT_ACCEPTED; break; @@ -863,7 +850,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_MISSION_START: - rover.set_mode(AUTO); + rover.set_mode(rover.mode_auto); result = MAV_RESULT_ACCEPTED; break; @@ -920,19 +907,19 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) switch (static_cast(packet.param1)) { case MAV_MODE_MANUAL_ARMED: case MAV_MODE_MANUAL_DISARMED: - rover.set_mode(MANUAL); + rover.set_mode(rover.mode_manual); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_AUTO_ARMED: case MAV_MODE_AUTO_DISARMED: - rover.set_mode(AUTO); + rover.set_mode(rover.mode_auto); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_STABILIZE_DISARMED: case MAV_MODE_STABILIZE_ARMED: - rover.set_mode(LEARNING); + rover.set_mode(rover.mode_learning); result = MAV_RESULT_ACCEPTED; break; @@ -1014,11 +1001,11 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) // param2 : Speed - normalized to 0 .. 1 // exit if vehicle is not in Guided mode - if (rover.control_mode != GUIDED) { + if (rover.control_mode != &rover.mode_guided) { break; } - rover.guided_mode = Guided_Angle; + rover.mode_guided.guided_mode = ModeGuided::Guided_Angle; rover.guided_control.msg_time_ms = AP_HAL::millis(); rover.guided_control.turn_angle = packet.param1; rover.guided_control.target_speed = constrain_float(packet.param2, 0.0f, 1.0f); @@ -1132,7 +1119,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) mavlink_msg_set_attitude_target_decode(msg, &packet); // exit if vehicle is not in Guided mode - if (rover.control_mode != GUIDED) { + if (rover.control_mode != &rover.mode_guided) { break; } // record the time when the last message arrived @@ -1173,7 +1160,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) mavlink_msg_set_position_target_local_ned_decode(msg, &packet); // exit if vehicle is not in Guided mode - if (rover.control_mode != GUIDED) { + if (rover.control_mode != &rover.mode_guided) { break; } @@ -1258,7 +1245,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) mavlink_msg_set_position_target_global_int_decode(msg, &packet); // exit if vehicle is not in Guided mode - if (rover.control_mode != GUIDED) { + if (rover.control_mode != &rover.mode_guided) { break; } // check for supported coordinate frames diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index 0a48260351..608c9bf4f9 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -197,7 +197,7 @@ void Rover::Log_Write_Steering() struct log_Steering pkt = { LOG_PACKET_HEADER_INIT(LOG_STEERING_MSG), time_us : AP_HAL::micros64(), - demanded_accel : lateral_acceleration, + demanded_accel : control_mode->lateral_acceleration, achieved_accel : ahrs.groundspeed() * ins.get_gyro().z, }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); @@ -329,7 +329,7 @@ void Rover::Log_Write_Rangefinder() struct log_Rangefinder pkt = { LOG_PACKET_HEADER_INIT(LOG_RANGEFINDER_MSG), time_us : AP_HAL::micros64(), - lateral_accel : lateral_acceleration, + lateral_accel : control_mode->lateral_acceleration, rangefinder1_distance : rangefinder.distance_cm(0), rangefinder2_distance : rangefinder.distance_cm(1), detected_count : obstacle.detected_count, @@ -526,7 +526,7 @@ void Rover::Log_Write_Vehicle_Startup_Messages() { // only 200(?) bytes are guaranteed by DataFlash Log_Write_Startup(TYPE_GROUNDSTART_MSG); - DataFlash.Log_Write_Mode(control_mode); + DataFlash.Log_Write_Mode(control_mode->mode_number()); Log_Write_Home_And_Origin(); gps.Write_DataFlash_Log_Startup_messages(); } diff --git a/APMrover2/Rover.cpp b/APMrover2/Rover.cpp index 17de33b35e..c418a8a65e 100644 --- a/APMrover2/Rover.cpp +++ b/APMrover2/Rover.cpp @@ -40,7 +40,7 @@ Rover::Rover(void) : #if MOUNT == ENABLED camera_mount(ahrs, current_loc), #endif - control_mode(INITIALISING), + control_mode(&mode_initializing), throttle(500), #if FRSKY_TELEM_ENABLED == ENABLED frsky_telemetry(ahrs, battery, rangefinder), diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index abc1e7acfa..108916284c 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -65,6 +65,8 @@ #include #include "AP_MotorsUGV.h" +#include "mode.h" + #include "AP_Arming.h" #include "compat.h" @@ -106,6 +108,13 @@ public: friend class AP_AdvancedFailsafe_Rover; #endif friend class GCS_Rover; + friend class Mode; + friend class ModeAuto; + friend class ModeGuided; + friend class ModeHold; + friend class ModeSteering; + friend class ModeManual; + friend class ModeRTL; Rover(void); @@ -221,10 +230,9 @@ private: // if USB is connected bool usb_connected; - // Radio // This is the state of the flight control system - // There are multiple states defined such as MANUAL, FBW-A, AUTO - enum mode control_mode; + // There are multiple states defined such as MANUAL, AUTO, ... + Mode *control_mode; // Used to maintain the state of the previous control switch position // This is set to -1 when we need to re-read the switch @@ -284,9 +292,6 @@ private: uint32_t detected_time_ms; } obstacle; - // this is set to true when auto has been triggered to start - bool auto_triggered; - // Ground speed // The amount current ground speed is below min ground speed. meters per second float ground_speed; @@ -310,10 +315,6 @@ private: uint32_t control_sensors_enabled; uint32_t control_sensors_health; - // Navigation control variables - // The instantaneous desired lateral acceleration in m/s/s - float lateral_acceleration; - // Waypoint distances // Distance between rover and next waypoint. Meters float wp_distance; @@ -399,12 +400,6 @@ private: // time that rudder/steering arming has been running uint32_t rudder_arm_timer; - // true if we are in an auto-throttle mode, which means - // we need to run the speed controller - bool auto_throttle_mode; - - // Guided control - GuidedMode guided_mode; // controls which controller is run (waypoint or velocity) // Store parameters from Guided msg struct { float turn_angle; // target heading in centi-degrees @@ -422,6 +417,15 @@ private: // True when we are doing motor test bool motor_test; + ModeInitializing mode_initializing; + ModeHold mode_hold; + ModeManual mode_manual; + ModeGuided mode_guided; + ModeAuto mode_auto; + ModeLearning mode_learning; + ModeSteering mode_steering; + ModeRTL mode_rtl; + private: // private member functions void ahrs_update(); @@ -458,6 +462,10 @@ private: void gcs_update(void); void gcs_retry_deferred(void); + Mode *control_mode_from_num(enum mode num); + bool set_mode(Mode &mode); + bool mavlink_set_mode(uint8_t mode); + void do_erase_logs(void); void Log_Write_Performance(); void Log_Write_Steering(); @@ -480,11 +488,7 @@ private: void Log_Arm_Disarm(); void load_parameters(void); - bool auto_check_trigger(void); bool use_pivot_steering(void); - void calc_throttle(float target_speed); - void calc_lateral_acceleration(); - void calc_nav_steer(); void set_servos(void); void set_auto_WP(const struct Location& loc); void set_guided_WP(const struct Location& loc); @@ -538,8 +542,7 @@ private: void init_ardupilot(); void startup_ground(void); void set_reverse(bool reverse); - void set_mode(enum mode mode); - bool mavlink_set_mode(uint8_t mode); + void failsafe_trigger(uint8_t failsafe_type, bool on); void startup_INS_ground(void); void update_notify(); diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp index 93e5764f47..de20697530 100644 --- a/APMrover2/Steering.cpp +++ b/APMrover2/Steering.cpp @@ -1,58 +1,12 @@ #include "Rover.h" -/* - check for triggering of start of auto mode -*/ -bool Rover::auto_check_trigger(void) { - // only applies to AUTO mode - if (control_mode != AUTO) { - return true; - } - - // check for user pressing the auto trigger to off - if (auto_triggered && g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 1) { - gcs().send_text(MAV_SEVERITY_WARNING, "AUTO triggered off"); - auto_triggered = false; - return false; - } - - // if already triggered, then return true, so you don't - // need to hold the switch down - if (auto_triggered) { - return true; - } - - if (g.auto_trigger_pin == -1 && is_zero(g.auto_kickstart)) { - // no trigger configured - let's go! - auto_triggered = true; - return true; - } - - if (g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 0) { - gcs().send_text(MAV_SEVERITY_WARNING, "Triggered AUTO with pin"); - auto_triggered = true; - return true; - } - - if (!is_zero(g.auto_kickstart)) { - const float xaccel = ins.get_accel().x; - if (xaccel >= g.auto_kickstart) { - gcs().send_text(MAV_SEVERITY_WARNING, "Triggered AUTO xaccel=%.1f", static_cast(xaccel)); - auto_triggered = true; - return true; - } - } - - return false; -} - /* work out if we are going to use pivot steering */ bool Rover::use_pivot_steering(void) { // check cases where we clearly cannot use pivot steering - if (control_mode < AUTO || !g2.motors.have_skid_steering() || g.pivot_turn_angle <= 0) { + if (control_mode->is_autopilot_mode() || !g2.motors.have_skid_steering() || g.pivot_turn_angle <= 0) { pivot_steering_active = false; return false; } @@ -82,7 +36,7 @@ bool Rover::use_pivot_steering(void) bool Rover::in_stationary_loiter() { // Confirm we are in AUTO mode and need to loiter for a time period - if ((loiter_start_time > 0) && (control_mode == AUTO)) { + if ((loiter_start_time > 0) && (control_mode == &mode_auto)) { // Check if active loiter is enabled AND we are outside the waypoint loiter radius // then the vehicle still needs to move so return false if (active_loiter && (wp_distance > g.waypoint_radius)) { @@ -94,156 +48,12 @@ bool Rover::in_stationary_loiter() return false; } -/* - calculate the throtte for auto-throttle modes -*/ -void Rover::calc_throttle(float target_speed) { - // If not autostarting OR we are loitering at a waypoint - // then set the throttle to minimum - if (!auto_check_trigger() || in_stationary_loiter()) { - g2.motors.set_throttle(g.throttle_min.get()); - // Stop rotation in case of loitering and skid steering - if (g2.motors.have_skid_steering()) { - g2.motors.set_steering(0.0f); - } - return; - } - - const float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise; - const int throttle_target = throttle_base + throttle_nudge; - - /* - reduce target speed in proportion to turning rate, up to the - SPEED_TURN_GAIN percentage. - */ - float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g*GRAVITY_MSS)); - steer_rate = constrain_float(steer_rate, 0.0f, 1.0f); - - // use g.speed_turn_gain for a 90 degree turn, and in proportion - // for other turn angles - const int32_t turn_angle = wrap_180_cd(next_navigation_leg_cd - ahrs.yaw_sensor); - const float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0.0f, 1.0f); - const float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f; - - float reduction = 1.0f - steer_rate * speed_turn_reduction; - - if (control_mode >= AUTO && guided_mode != Guided_Velocity && wp_distance <= g.speed_turn_dist) { - // in auto-modes we reduce speed when approaching waypoints - const float reduction2 = 1.0f - speed_turn_reduction; - if (reduction2 < reduction) { - reduction = reduction2; - } - } - - // reduce the target speed by the reduction factor - target_speed *= reduction; - - groundspeed_error = fabsf(target_speed) - ground_speed; - - throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100.0f) / 100.0f); - - // also reduce the throttle by the reduction factor. This gives a - // much faster response in turns - throttle *= reduction; - - if (in_reverse) { - g2.motors.set_throttle(constrain_int16(-throttle, -g.throttle_max, -g.throttle_min)); - } else { - g2.motors.set_throttle(constrain_int16(throttle, g.throttle_min, g.throttle_max)); - } - - if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) { - // the user has asked to use reverse throttle to brake. Apply - // it in proportion to the ground speed error, but only when - // our ground speed error is more than BRAKING_SPEEDERR. - // - // We use a linear gain, with 0 gain at a ground speed error - // of braking_speederr, and 100% gain when groundspeed_error - // is 2*braking_speederr - const float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0.0f, 1.0f); - const int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain; - g2.motors.set_throttle(constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min)); - - // temporarily set us in reverse to allow the PWM setting to - // go negative - set_reverse(true); - } - - if (guided_mode != Guided_Velocity) { - if (use_pivot_steering()) { - // In Guided Velocity, only the steering input is used to calculate the pivot turn. - g2.motors.set_throttle(0.0f); - } - } -} - -/***************************************** - Calculate desired turn angles (in medium freq loop) - *****************************************/ - -void Rover::calc_lateral_acceleration() { - switch (control_mode) { - case AUTO: - // If we have reached the waypoint previously navigate - // back to it from our current position - if (previously_reached_wp && (loiter_duration > 0)) { - nav_controller->update_waypoint(current_loc, next_WP); - } else { - nav_controller->update_waypoint(prev_WP, next_WP); - } - break; - - case RTL: - case GUIDED: - case STEERING: - nav_controller->update_waypoint(current_loc, next_WP); - break; - default: - return; - } - - // Calculate the required turn of the wheels - - // negative error = left turn - // positive error = right turn - lateral_acceleration = nav_controller->lateral_acceleration(); - if (use_pivot_steering()) { - const int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100; - if (bearing_error > 0) { - lateral_acceleration = g.turn_max_g * GRAVITY_MSS; - } else { - lateral_acceleration = -g.turn_max_g * GRAVITY_MSS; - } - } -} - -/* - calculate steering angle given lateral_acceleration -*/ -void Rover::calc_nav_steer() { - // check to see if the rover is loitering - if (in_stationary_loiter()) { - g2.motors.set_steering(0.0f); - return; - } - - // add in obstacle avoidance - if (!in_reverse) { - lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g; - } - - // constrain to max G force - lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g * GRAVITY_MSS, g.turn_max_g * GRAVITY_MSS); - - g2.motors.set_steering(steerController.get_steering_out_lat_accel(lateral_acceleration)); -} - /***************************************** Set the flight control servos based on the current calculated values *****************************************/ void Rover::set_servos(void) { // Apply slew rate limit on non Manual modes - if (control_mode == MANUAL || control_mode == LEARNING) { + if (control_mode == &mode_manual || control_mode == &mode_learning) { g2.motors.slew_limit_throttle(false); if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) { g2.motors.set_throttle(0.0f); diff --git a/APMrover2/commands.cpp b/APMrover2/commands.cpp index ce8c550390..3610d0e87b 100644 --- a/APMrover2/commands.cpp +++ b/APMrover2/commands.cpp @@ -29,7 +29,7 @@ void Rover::set_auto_WP(const struct Location& loc) void Rover::set_guided_WP(const struct Location& loc) { - guided_mode = Guided_WP; + rover.mode_guided.guided_mode = ModeGuided::Guided_WP; // copy the current location into the OldWP slot // --------------------------------------- prev_WP = current_loc; @@ -47,12 +47,11 @@ void Rover::set_guided_WP(const struct Location& loc) void Rover::set_guided_velocity(float target_steer_speed, float target_speed) { - guided_mode = Guided_Velocity; + rover.mode_guided.guided_mode = ModeGuided::Guided_Velocity; rover.guided_control.target_steer_speed = target_steer_speed; rover.guided_control.target_speed = target_speed; next_WP = current_loc; - lateral_acceleration = 0.0f; // this is handy for the groundstation wp_totalDistance = 0; wp_distance = 0.0f; diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index 7f47ffea8d..741a8cc44d 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -11,7 +11,7 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd) } // exit immediately if not in AUTO mode - if (control_mode != AUTO) { + if (control_mode != &mode_auto) { return false; } @@ -130,9 +130,9 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd) // we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode void Rover::exit_mission() { - if (control_mode == AUTO) { + if (control_mode == &mode_auto) { gcs().send_text(MAV_SEVERITY_NOTICE, "No commands. Can't set AUTO. Setting HOLD"); - set_mode(HOLD); + set_mode(mode_hold); } } @@ -140,7 +140,7 @@ void Rover::exit_mission() // we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode bool Rover::verify_command_callback(const AP_Mission::Mission_Command& cmd) { - if (control_mode == AUTO) { + if (control_mode == &mode_auto) { const bool cmd_complete = verify_command(cmd); // send message to GCS @@ -216,7 +216,7 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd) void Rover::do_RTL(void) { prev_WP = current_loc; - control_mode = RTL; + control_mode = &mode_rtl; next_WP = home; } @@ -379,7 +379,6 @@ void Rover::nav_set_yaw_speed() gcs().send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping"); g2.motors.set_throttle(g.throttle_min.get()); g2.motors.set_steering(0.0f); - lateral_acceleration = 0.0f; return; } @@ -390,9 +389,9 @@ void Rover::nav_set_yaw_speed() // 0.5 would set speed to the cruise speed // 1 is double the cruise speed. const float target_speed = g.speed_cruise * guided_control.target_speed * 2.0f; - calc_throttle(target_speed); + rover.control_mode->calc_throttle(target_speed); - Log_Write_GuidedTarget(guided_mode, Vector3f(steering, 0.0f, 0.0f), Vector3f(target_speed, 0.0f, 0.0f)); + Log_Write_GuidedTarget(rover.mode_guided.guided_mode, Vector3f(steering, 0.0f, 0.0f), Vector3f(target_speed, 0.0f, 0.0f)); } void Rover::nav_set_speed() @@ -402,7 +401,6 @@ void Rover::nav_set_speed() gcs().send_text(MAV_SEVERITY_WARNING, "SET_VELOCITY not recvd last 3secs, stopping"); g2.motors.set_throttle(g.throttle_min.get()); g2.motors.set_steering(0.0f); - lateral_acceleration = 0.0f; prev_WP = current_loc; next_WP = current_loc; set_guided_WP(current_loc); // exit Guided_Velocity to prevent spam @@ -416,9 +414,9 @@ void Rover::nav_set_speed() nav_controller->update_waypoint(current_loc, next_WP); g2.motors.set_steering(steer_value); - calc_throttle(guided_control.target_speed); + rover.control_mode->calc_throttle(guided_control.target_speed); - Log_Write_GuidedTarget(guided_mode, Vector3f(steer_value, 0.0f, 0.0f), Vector3f(guided_control.target_speed, 0.0f, 0.0f)); + Log_Write_GuidedTarget(rover.mode_guided.guided_mode, Vector3f(steer_value, 0.0f, 0.0f), Vector3f(guided_control.target_speed, 0.0f, 0.0f)); } /********************************************************************************/ @@ -461,7 +459,7 @@ void Rover::do_yaw(const AP_Mission::Mission_Command& cmd) const int32_t steering = steerController.get_steering_out_angle_error(error_to_target_yaw); g2.motors.set_steering(steering); next_navigation_leg_cd = condition_value; - calc_throttle(g.speed_cruise); + control_mode->calc_throttle(g.speed_cruise); do_auto_rotation = true; } @@ -483,7 +481,7 @@ bool Rover::do_yaw_rotation() // Calculate the steering to apply base on error calculated const int32_t steering = steerController.get_steering_out_angle_error(error_to_target_yaw); g2.motors.set_steering(steering); - calc_throttle(g.speed_cruise); + control_mode->calc_throttle(g.speed_cruise); do_auto_rotation = true; return false; } diff --git a/APMrover2/commands_process.cpp b/APMrover2/commands_process.cpp index 126d822e38..4c6249c2d8 100644 --- a/APMrover2/commands_process.cpp +++ b/APMrover2/commands_process.cpp @@ -4,7 +4,7 @@ // -------------------- void Rover::update_commands(void) { - if (control_mode == AUTO) { + if (control_mode == &mode_auto) { if (home_is_set != HOME_UNSET && mission.num_commands() > 1) { mission.update(); } diff --git a/APMrover2/control_modes.cpp b/APMrover2/control_modes.cpp index 8e371823c0..e18f1884bc 100644 --- a/APMrover2/control_modes.cpp +++ b/APMrover2/control_modes.cpp @@ -2,6 +2,40 @@ static const int16_t CH_7_PWM_TRIGGER = 1800; +Mode *Rover::control_mode_from_num(const enum mode num) +{ + Mode *ret = nullptr; + switch (num) { + case MANUAL: + ret = &mode_manual; + break; + case LEARNING: + ret = &mode_learning; + break; + case STEERING: + ret = &mode_steering; + break; + case HOLD: + ret = &mode_hold; + break; + case AUTO: + ret = &mode_auto; + break; + case RTL: + ret = &mode_rtl; + break; + case GUIDED: + ret = &mode_guided; + break; + case INITIALISING: + ret = &mode_initializing; + break; + default: + break; + } + return ret; +} + void Rover::read_control_switch() { static bool switch_debouncer; @@ -36,7 +70,10 @@ void Rover::read_control_switch() return; } - set_mode((enum mode)modes[switchPosition].get()); + Mode *new_mode = control_mode_from_num((enum mode)modes[switchPosition].get()); + if (new_mode != nullptr) { + set_mode(*new_mode); + } oldSwitchPosition = switchPosition; prev_WP = current_loc; @@ -92,8 +129,7 @@ void Rover::read_trim_switch() if (ch7_flag) { ch7_flag = false; - switch (control_mode) { - case MANUAL: + if (control_mode == &mode_manual) { hal.console->printf("Erasing waypoints\n"); // if SW7 is ON in MANUAL = Erase the Flight Plan mission.clear(); @@ -101,10 +137,7 @@ void Rover::read_trim_switch() // if roll is full right store the current location as home set_home_to_current_location(false); } - break; - - case LEARNING: - case STEERING: { + } else if (control_mode == &mode_learning || control_mode == &mode_steering) { // if SW7 is ON in LEARNING = record the Wp // create new mission command @@ -120,14 +153,9 @@ void Rover::read_trim_switch() if (mission.add_cmd(cmd)) { hal.console->printf("Learning waypoint %u", static_cast(mission.num_commands())); } - break; - } - case AUTO: + } else if (control_mode == &mode_auto) { // if SW7 is ON in AUTO = set to RTL - set_mode(RTL); - break; - - default: + set_mode(mode_rtl); break; } } diff --git a/APMrover2/crash_check.cpp b/APMrover2/crash_check.cpp index 2aae28925b..bea241f496 100644 --- a/APMrover2/crash_check.cpp +++ b/APMrover2/crash_check.cpp @@ -13,7 +13,7 @@ void Rover::crash_check() static uint16_t crash_counter; // number of iterations vehicle may have been crashed // return immediately if disarmed, or crash checking disabled or in HOLD mode - if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || (control_mode != GUIDED && control_mode != AUTO)) { + if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || (!control_mode->is_autopilot_mode())) { crash_counter = 0; return; } @@ -38,7 +38,7 @@ void Rover::crash_check() // send message to gcs gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD"); // change mode to hold and disarm - set_mode(HOLD); + set_mode(mode_hold); if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) { disarm_motors(); } diff --git a/APMrover2/defines.h b/APMrover2/defines.h index aa2cb20e10..217068bea7 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -40,12 +40,6 @@ enum mode { INITIALISING = 16 }; -enum GuidedMode { - Guided_WP, - Guided_Angle, - Guided_Velocity -}; - // types of failsafe events #define FAILSAFE_EVENT_THROTTLE (1<<0) #define FAILSAFE_EVENT_GCS (1<<1) diff --git a/APMrover2/failsafe.cpp b/APMrover2/failsafe.cpp index 9ed9d313b7..ae9adb83a8 100644 --- a/APMrover2/failsafe.cpp +++ b/APMrover2/failsafe.cpp @@ -74,18 +74,18 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on) if (failsafe.triggered == 0 && failsafe.bits != 0 && millis() - failsafe.start_time > g.fs_timeout * 1000 && - control_mode != RTL && - control_mode != HOLD) { + control_mode != &mode_rtl && + control_mode != &mode_hold) { failsafe.triggered = failsafe.bits; gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe trigger 0x%x", static_cast(failsafe.triggered)); switch (g.fs_action) { case 0: break; case 1: - set_mode(RTL); + set_mode(mode_rtl); break; case 2: - set_mode(HOLD); + set_mode(mode_hold); break; } } diff --git a/APMrover2/radio.cpp b/APMrover2/radio.cpp index 492416ab0e..84cbeff9b8 100644 --- a/APMrover2/radio.cpp +++ b/APMrover2/radio.cpp @@ -53,7 +53,7 @@ void Rover::rudder_arm_disarm_check() } // if not in a manual throttle mode then disallow rudder arming/disarming - if (auto_throttle_mode) { + if (control_mode->auto_throttle()) { rudder_arm_timer = 0; return; } diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp index 8173bd79cc..c485b08160 100644 --- a/APMrover2/sensors.cpp +++ b/APMrover2/sensors.cpp @@ -239,29 +239,13 @@ void Rover::update_sensor_status_flags(void) ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & ~MAV_SYS_STATUS_LOGGING); - - switch (control_mode) { - case MANUAL: - case HOLD: - break; - - case LEARNING: - case STEERING: - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation - break; - - case AUTO: - case RTL: - case GUIDED: - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control - break; - - case INITIALISING: - break; + if (control_mode->attitude_stabilized()) { + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // 3D angular rate control + } + if (control_mode->is_autopilot_mode()) { + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control } if (rover.DataFlash.logging_enabled()) { diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 2a342a8147..510edbd1f4 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -121,7 +121,7 @@ void Rover::init_ardupilot() // initialise notify system notify.init(false); AP_Notify::flags.failsafe_battery = false; - notify_mode(control_mode); + notify_mode((enum mode)control_mode->mode_number()); ServoRelayEvents.set_channel_mask(0xFFF0); @@ -209,7 +209,12 @@ void Rover::init_ardupilot() startup_ground(); - set_mode((enum mode)g.initial_mode.get()); + Mode *initial_mode = control_mode_from_num((enum mode)g.initial_mode.get()); + if (initial_mode == nullptr) { + initial_mode = &mode_initializing; + } + set_mode(*initial_mode); + // set the correct flight mode // --------------------------- @@ -227,7 +232,7 @@ void Rover::init_ardupilot() //********************************************************************************* void Rover::startup_ground(void) { - set_mode(INITIALISING); + set_mode(mode_initializing); gcs().send_text(MAV_SEVERITY_INFO, " Ground start"); @@ -278,75 +283,32 @@ void Rover::set_reverse(bool reverse) in_reverse = reverse; } -void Rover::set_mode(enum mode mode) +bool Rover::set_mode(Mode &new_mode) { - if (control_mode == mode) { + if (control_mode == &new_mode) { // don't switch modes if we are already in the correct mode. - return; + return true; } - // If we are changing out of AUTO mode reset the loiter timer and stop current mission - if (control_mode == AUTO) { - loiter_start_time = 0; - if (mission.state() == AP_Mission::MISSION_RUNNING) { - mission.stop(); - } + Mode &old_mode = *control_mode; + if (!new_mode.enter()) { + return false; } - control_mode = mode; - throttle = 500; - if (!in_auto_reverse) { - set_reverse(false); - } - g.pidSpeedThrottle.reset_I(); + control_mode = &new_mode; #if FRSKY_TELEM_ENABLED == ENABLED - frsky_telemetry.update_control_mode(control_mode); + frsky_telemetry.update_control_mode(control_mode->mode_number()); #endif - if (control_mode != AUTO) { - auto_triggered = false; - } - - switch (control_mode) { - case MANUAL: - case HOLD: - case LEARNING: - case STEERING: - auto_throttle_mode = false; - break; - - case AUTO: - auto_throttle_mode = true; - rtl_complete = false; - restart_nav(); - break; - - case RTL: - auto_throttle_mode = true; - do_RTL(); - break; - - case GUIDED: - auto_throttle_mode = true; - /* - when entering guided mode we set the target as the current - location. This matches the behaviour of the copter code. - */ - set_guided_WP(current_loc); - break; - - default: - auto_throttle_mode = true; - do_RTL(); - break; - } + old_mode.exit(); if (should_log(MASK_LOG_MODE)) { - DataFlash.Log_Write_Mode(control_mode); + DataFlash.Log_Write_Mode(control_mode->mode_number()); } - notify_mode(control_mode); + notify_mode((enum mode)control_mode->mode_number()); + return true; } /* @@ -354,18 +316,11 @@ void Rover::set_mode(enum mode mode) */ bool Rover::mavlink_set_mode(uint8_t mode) { - switch (mode) { - case MANUAL: - case HOLD: - case LEARNING: - case STEERING: - case GUIDED: - case AUTO: - case RTL: - set_mode((enum mode)mode); - return true; + Mode *new_mode = control_mode_from_num((enum mode)mode); + if (new_mode == nullptr) { + return false; } - return false; + return set_mode(*new_mode); } void Rover::startup_INS_ground(void) @@ -532,7 +487,7 @@ bool Rover::disarm_motors(void) if (!arming.disarm()) { return false; } - if (control_mode != AUTO) { + if (control_mode != &mode_auto) { // reset the mission on disarm if we are not in auto mission.reset(); }