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

Rover: added a new form of GUIDED mode 

Rover now accepts a new message MAV_CMD_NAV_SET_YAW_SPEED
which has an angle in centidegrees and a speed scale and the rover
will drive based on these inputs.
This commit is contained in:
Grant Morphett 2016-09-15 22:09:45 +10:00
parent 79cb4ea595
commit 2d09db4ecb
5 changed files with 110 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
APMrover2/APMrover2.cpp
View File

@ -412,21 +412,34 @@ void Rover::update_current_mode(void)
calc_throttle(g.speed_cruise);
break;
case GUIDED:
if (rtl_complete || verify_RTL()) {
// we have reached destination so stop where we are
if (channel_throttle->get_servo_out() != g.throttle_min.get()) {
gcs_send_mission_item_reached_message(0);
case GUIDED: {
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 (channel_throttle->get_servo_out() != g.throttle_min.get()) {
gcs_send_mission_item_reached_message(0);
}
channel_throttle->set_servo_out(g.throttle_min.get());
channel_steer->set_servo_out(0);
lateral_acceleration = 0;
} else {
calc_lateral_acceleration();
calc_nav_steer();
calc_throttle(g.speed_cruise);
}
channel_throttle->set_servo_out(g.throttle_min.get());
channel_steer->set_servo_out(0);
lateral_acceleration = 0;
} else {
calc_lateral_acceleration();
calc_nav_steer();
calc_throttle(g.speed_cruise);
break;
default:
gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break;
}
break;
}
case STEERING: {
/*
@ -509,17 +522,29 @@ void Rover::update_navigation()
break;
case GUIDED:
// no loitering around the wp with the rover, goes direct to the wp position
calc_lateral_acceleration();
calc_nav_steer();
if (rtl_complete || verify_RTL()) {
// we have reached destination so stop where we are
channel_throttle->set_servo_out(g.throttle_min.get());
channel_steer->set_servo_out(0);
lateral_acceleration = 0;
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
calc_lateral_acceleration();
calc_nav_steer();
if (rtl_complete || verify_RTL()) {
// we have reached destination so stop where we are
channel_throttle->set_servo_out(g.throttle_min.get());
channel_steer->set_servo_out(0);
lateral_acceleration = 0;
}
break;
default:
gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break;
}
break;
}
}
AP_HAL_MAIN_CALLBACKS(&rover);

23
APMrover2/GCS_Mavlink.cpp
View File

@ -796,9 +796,11 @@ bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd)
// only accept position updates when in GUIDED mode
return false;
}
rover.guided_WP = cmd.content.location;
// 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.rtl_complete = false;
rover.set_guided_WP();
@ -1166,6 +1168,25 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
result = rover.compass.handle_mag_cal_command(packet);
break;
case MAV_CMD_NAV_SET_YAW_SPEED:
{
// param1 : yaw angle to adjust direction by in centidegress
// param2 : Speed - normalized to 0 .. 1
// exit if vehicle is not in Guided mode
if (rover.control_mode != GUIDED) {
break;
}
rover.guided_mode = Guided_Angle;
rover.guided_yaw_speed.msg_time_ms = AP_HAL::millis();
rover.guided_yaw_speed.turn_angle = packet.param1;
rover.guided_yaw_speed.target_speed = constrain_float(packet.param2, 0.0f, 1.0f);
rover.nav_set_yaw_speed();
result = MAV_RESULT_ACCEPTED;
break;
}
default:
break;
}

14
APMrover2/Rover.h
View File

@ -369,7 +369,18 @@ private:
bool auto_throttle_mode;
// Store the time the last GPS message was received.
uint32_t last_gps_msg_ms{0};
uint32_t last_gps_msg_ms{0};
// Store parameters from NAV_SET_YAW_SPEED
struct {
float turn_angle;
float target_speed;
uint32_t msg_time_ms;
} guided_yaw_speed;
// Guided
GuidedMode guided_mode; // stores which GUIDED mode the vehicle is in
private:
// private member functions
@ -520,6 +531,7 @@ private:
bool motor_active();
void update_home();
void accel_cal_update(void);
void nav_set_yaw_speed();
public:
bool print_log_menu(void);
int8_t dump_log(uint8_t argc, const Menu::arg *argv);

23
APMrover2/commands_logic.cpp
View File

@ -307,6 +307,29 @@ bool Rover::verify_loiter_unlim()
return false;
}
void Rover::nav_set_yaw_speed()
{
// if we haven't received a MAV_CMD_NAV_SET_YAW_SPEED message within the last 3 seconds bring the rover to a halt
if ((millis() - guided_yaw_speed.msg_time_ms) > 3000)
{
gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping");
channel_throttle->set_servo_out(g.throttle_min.get());
channel_steer->set_servo_out(0);
lateral_acceleration = 0;
return;
}
channel_steer->set_servo_out(steerController.get_steering_out_angle_error(guided_yaw_speed.turn_angle));
// speed param in the message gives speed as a proportion of cruise speed.
// 0.5 would set speed to the cruise speed
// 1 is double the cruise speed.
float target_speed = g.speed_cruise * guided_yaw_speed.target_speed * 2;
calc_throttle(target_speed);
return;
}
/********************************************************************************/
// Condition (May) commands
/********************************************************************************/

6
APMrover2/defines.h
View File

@ -40,6 +40,12 @@ enum mode {
INITIALISING=16
};
enum GuidedMode {
Guided_WP,
Guided_Angle
};
// types of failsafe events
#define FAILSAFE_EVENT_THROTTLE (1<<0)
#define FAILSAFE_EVENT_GCS (1<<1)