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

Rover: add Guided velocity controller

This commit is contained in:
Pierre Kancir 2017-05-03 16:21:58 +02:00 committed by Grant Morphett
parent c1d3384835
commit 045d171ab9
7 changed files with 189 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
APMrover2/APMrover2.cpp
View File

@ -474,12 +474,16 @@ void Rover::update_current_mode(void)
} else {
calc_lateral_acceleration();
calc_nav_steer();
calc_throttle(g.speed_cruise);
Log_Write_GuidedTarget(guided_mode, Vector3f(guided_WP.lat, guided_WP.lng, guided_WP.alt),
Vector3f(g.speed_cruise, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 0));
calc_throttle(guided_target_speed);
Log_Write_GuidedTarget(guided_mode, Vector3f(next_WP.lat, next_WP.lng, next_WP.alt),
Vector3f(guided_target_speed, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 0));
}
break;
case Guided_Velocity:
nav_set_speed();
break;
default:
gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break;
@ -591,6 +595,10 @@ void Rover::update_navigation()
}
break;
case Guided_Velocity:
nav_set_speed();
break;
default:
gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break;

125
APMrover2/GCS_Mavlink.cpp
View File

@ -1241,6 +1241,47 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
break;
}
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: // MAV ID: 82
{
// decode packet
mavlink_set_attitude_target_t packet;
mavlink_msg_set_attitude_target_decode(msg, &packet);
// exit if vehicle is not in Guided mode
if (rover.control_mode != GUIDED) {
break;
}
// record the time when the last message arrived
rover.guided_yaw_speed.msg_time_ms = AP_HAL::millis();
// ensure type_mask specifies to use thrust
if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE) != 0) {
break;
}
// convert thrust to ground speed
packet.thrust = constrain_float(packet.thrust, 0.0f, 1.0f);
float target_speed = 0.0f;
if (is_equal(packet.thrust, 0.5f)) {
target_speed = 0.0f;
} else if (packet.thrust > 0.5f) {
target_speed = (packet.thrust - 0.5f) * 2.0f * rover.g.speed_cruise;
} else {
target_speed = (0.5f - packet.thrust) * 2.0f * rover.g.speed_cruise;
}
// if the body_yaw_rate field is ignored, use the commanded yaw position
// otherwise use the commanded yaw rate
if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_YAW_RATE_IGNORE) != 0) {
// convert quaternion to heading
// int32_t target_heading_cd = static_cast<int32_t>(degrees(Quaternion(packet.q[0], packet.q[1], packet.q[2], packet.q[3]).get_euler_yaw()) * 100);
// TODO : handle that
} else {
rover.set_guided_velocity((RAD_TO_DEG * packet.body_yaw_rate), target_speed);
}
break;
}
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84
{
// decode packet
@ -1261,10 +1302,15 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
}
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
// record the time when the last message arrived
rover.guided_yaw_speed.msg_time_ms = AP_HAL::millis();
// prepare and send target position
Location target_loc = rover.current_loc;
if (!pos_ignore) {
Location loc = rover.current_loc;
switch (packet.coordinate_frame) {
case MAV_FRAME_BODY_NED:
case MAV_FRAME_BODY_OFFSET_NED: {
@ -1272,25 +1318,52 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
float ne_x = packet.x*rover.ahrs.cos_yaw() - packet.y*rover.ahrs.sin_yaw();
float ne_y = packet.x*rover.ahrs.sin_yaw() + packet.y*rover.ahrs.cos_yaw();
// add offset to current location
location_offset(loc, ne_x, ne_y);
location_offset(target_loc, ne_x, ne_y);
}
break;
case MAV_FRAME_LOCAL_OFFSET_NED:
// add offset to current location
location_offset(loc, packet.x, packet.y);
location_offset(target_loc, packet.x, packet.y);
break;
default:
// MAV_FRAME_LOCAL_NED interpret as an offset from home
loc = rover.ahrs.get_home();
location_offset(loc, packet.x, packet.y);
target_loc = rover.ahrs.get_home();
location_offset(target_loc, packet.x, packet.y);
break;
}
rover.set_guided_WP(loc);
}
float target_speed = 0.0f;
float target_steer_speed = 0.0f;
if (!vel_ignore) {
// use packet vy (forward in NED) for speed in m/s and packet yaw_rate for turn in rad/s
float velx = packet.vx;
float vely = packet.vy;
// rotate to body-frame if necessary
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
// rotate from body-frame to NE frame
velx = velx * rover.ahrs.cos_yaw() - vely * rover.ahrs.sin_yaw();
vely = velx * rover.ahrs.sin_yaw() + vely * rover.ahrs.cos_yaw();
}
target_speed = vely;
target_steer_speed = RAD_TO_DEG * packet.yaw_rate;
// TODO : take into account reverse speed
// TODO : handle yaw heading cmd
}
if (!pos_ignore && !vel_ignore && acc_ignore) {
rover.set_guided_WP(target_loc);
if (!is_zero(target_speed)) {
rover.guided_target_speed = target_speed;
}
} else if (pos_ignore && !vel_ignore && acc_ignore) {
rover.set_guided_velocity(target_steer_speed, target_speed);
} else if (!pos_ignore && vel_ignore && acc_ignore) {
rover.set_guided_WP(target_loc);
} else {
// result = MAV_RESULT_FAILED; // TODO : support that
}
break;
}
@ -1314,15 +1387,45 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
}
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
// record the time where the last message arrived
rover.guided_yaw_speed.msg_time_ms = AP_HAL::millis();
// prepare and send target position
Location target_loc = rover.current_loc;
if (!pos_ignore) {
Location loc = rover.current_loc;
loc.lat = packet.lat_int;
loc.lng = packet.lon_int;
rover.set_guided_WP(loc);
// sanity check location
if (!check_latlng(packet.lat_int, packet.lon_int)) {
// result = MAV_RESULT_FAILED;
break;
}
target_loc.lat = packet.lat_int;
target_loc.lng = packet.lon_int;
}
float target_speed = 0.0f;
float target_steer_speed = 0.0f;
if (!vel_ignore) {
// use packet vy (forward in NED) for speed in m/s and packet yaw_rate for turn in rad/s
target_speed = packet.vy;
target_steer_speed = RAD_TO_DEG * packet.yaw_rate;
// TODO : take into account reverse speed
// TODO : handle yaw heading cmd
}
if (!pos_ignore && !vel_ignore && acc_ignore) {
rover.set_guided_WP(target_loc);
if (!is_zero(target_speed)) {
rover.guided_target_speed = target_speed;
}
} else if (pos_ignore && !vel_ignore && acc_ignore) {
rover.set_guided_velocity(target_steer_speed, target_speed);
} else if (!pos_ignore && vel_ignore && acc_ignore) {
rover.set_guided_WP(target_loc);
} else {
// result = MAV_RESULT_FAILED; // TODO : support that
}
break;
}

10
APMrover2/Rover.h
View File

@ -389,6 +389,11 @@ private:
// we need to run the speed controller
bool auto_throttle_mode;
// Guided control
GuidedMode guided_mode; // controls which controller is run (waypoint or velocity)
float guided_target_steer_speed; // target heading in centi-degrees
float guided_target_speed; // target speed in m/s
// Store the time the last GPS message was received.
uint32_t last_gps_msg_ms{0};
@ -399,9 +404,6 @@ private:
uint32_t msg_time_ms;
} guided_yaw_speed;
// Guided
GuidedMode guided_mode; // stores which GUIDED mode the vehicle is in
private:
// private member functions
void ahrs_update();
@ -471,6 +473,7 @@ private:
void set_servos(void);
void set_auto_WP(const struct Location& loc);
void set_guided_WP(const struct Location& loc);
void set_guided_velocity(float target_steer_speed, float target_speed);
void init_home();
void restart_nav();
void exit_mission();
@ -568,6 +571,7 @@ private:
void accel_cal_update(void);
void nav_set_yaw_speed();
bool do_yaw_rotation();
void nav_set_speed();
bool in_stationary_loiter(void);
void set_loiter_active(const AP_Mission::Mission_Command& cmd);
void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);

8
APMrover2/Steering.cpp
View File

@ -129,7 +129,7 @@ void Rover::calc_throttle(float target_speed) {
float reduction = 1.0f - steer_rate * speed_turn_reduction;
if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
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) {
@ -171,8 +171,10 @@ void Rover::calc_throttle(float target_speed) {
set_reverse(true);
}
if (use_pivot_steering()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
if (guided_mode != Guided_Velocity) {
if (use_pivot_steering()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
}
}
}

18
APMrover2/commands.cpp
View File

@ -39,6 +39,7 @@ void Rover::set_auto_WP(const struct Location& loc)
void Rover::set_guided_WP(const struct Location& loc)
{
guided_mode = Guided_WP;
// copy the current location into the OldWP slot
// ---------------------------------------
prev_WP = current_loc;
@ -46,7 +47,7 @@ void Rover::set_guided_WP(const struct Location& loc)
// Load the next_WP slot
// ---------------------
next_WP = loc;
guided_target_speed = g.speed_cruise;
// this is handy for the groundstation
wp_totalDistance = get_distance(current_loc, next_WP);
wp_distance = wp_totalDistance;
@ -54,6 +55,21 @@ void Rover::set_guided_WP(const struct Location& loc)
rover.rtl_complete = false;
}
void Rover::set_guided_velocity(float target_steer_speed, float target_speed)
{
guided_mode = Guided_Velocity;
guided_target_steer_speed = target_steer_speed;
guided_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;
rover.rtl_complete = false;
}
// run this at setup on the ground
// -------------------------------
void Rover::init_home()

26
APMrover2/commands_logic.cpp
View File

@ -396,6 +396,32 @@ void Rover::nav_set_yaw_speed()
return;
}
void Rover::nav_set_speed()
{
// if we haven't received a MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED 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, "SET_VELOCITY not recvd last 3secs, stopping");
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
lateral_acceleration = 0.0f;
prev_WP = current_loc;
next_WP = current_loc;
set_guided_WP(current_loc); // exit Guided_Velocity to prevent spam
return;
}
prev_WP = current_loc;
next_WP = current_loc;
const int32_t steer_value = steerController.get_steering_out_rate(guided_target_steer_speed);
location_update(next_WP, (steer_value + ahrs.yaw_sensor) * 0.01f, 4.0f); // put the next wp at 4m forward at steer direction
nav_controller->update_waypoint(current_loc, next_WP);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steer_value);
calc_throttle(guided_target_speed);
Log_Write_GuidedTarget(guided_mode, Vector3f(guided_target_steer_speed, 0.0f, 0.0f), Vector3f(guided_target_speed, 0.0f, 0.0f));
}
/********************************************************************************/
// Condition (May) commands
/********************************************************************************/

11
APMrover2/defines.h
View File

@ -42,10 +42,10 @@ enum mode {
enum GuidedMode {
Guided_WP,
Guided_Angle
Guided_Angle,
Guided_Velocity
};
// types of failsafe events
#define FAILSAFE_EVENT_THROTTLE (1<<0)
#define FAILSAFE_EVENT_GCS (1<<1)
@ -92,6 +92,13 @@ enum GuidedMode {
#define MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE (1<<10)
#define MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE (1<<11)
// for mavlink SET_ATTITUDE_TARGET messages
#define MAVLINK_SET_ATT_TYPE_MASK_ROLL_RATE_IGNORE (1<<0)
#define MAVLINK_SET_ATT_TYPE_MASK_PITCH_RATE_IGNORE (1<<1)
#define MAVLINK_SET_ATT_TYPE_MASK_YAW_RATE_IGNORE (1<<2)
#define MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE (1<<6)
#define MAVLINK_SET_ATT_TYPE_MASK_ATTITUDE_IGNORE (1<<7)
// Error message sub systems and error codes
#define ERROR_SUBSYSTEM_CRASH_CHECK 12
// subsystem specific error codes -- crash checker