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

Rover: integrate mode class

This commit is contained in:
Peter Barker 2017-07-19 11:19:08 +09:00 committed by Randy Mackay
parent 2a9b1017b6
commit 07f4603533
16 changed files with 160 additions and 556 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

160
APMrover2/APMrover2.cpp
View File

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

73
APMrover2/GCS_Mavlink.cpp
View File

@ -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<uint16_t>(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

6
APMrover2/Log.cpp
View File

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

2
APMrover2/Rover.cpp
View File

@ -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),

47
APMrover2/Rover.h
View File

@ -65,6 +65,8 @@
#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
#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();

196
APMrover2/Steering.cpp
View File

@ -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<double>(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);

5
APMrover2/commands.cpp
View File

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

24
APMrover2/commands_logic.cpp
View File

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

2
APMrover2/commands_process.cpp
View File

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

56
APMrover2/control_modes.cpp
View File

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

4
APMrover2/crash_check.cpp
View File

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

6
APMrover2/defines.h
View File

@ -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)

8
APMrover2/failsafe.cpp
View File

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

2
APMrover2/radio.cpp
View File

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

30
APMrover2/sensors.cpp
View File

@ -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()) {

95
APMrover2/system.cpp
View File

@ -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, "<startup_ground> 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();
}