aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: replace vehicle code guided methods with calls to guided mode class

Browse Source 
set_guided_WP replaced by mode_guided.set_desired_location
nav_set_yaw_speed replaced with mode_guided.set_desired_heading_and_speed
set_guided_velocity replaced with mode_guided.set_desired_turn_rate_and_speed
guided_control structure replaced with mode_guided members
use_pivot_steering accepts yaw-error argument instead of calculating it itself internally
This commit is contained in:
Randy Mackay 2017-08-02 08:10:38 +09:00
parent 783f8243df
commit b9db9229d9
5 changed files with 23 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
APMrover2/GCS_Mavlink.cpp
View File

@ -657,7 +657,7 @@ bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd)
}
// make any new wp uploaded instant (in case we are already in Guided mode)
rover.set_guided_WP(cmd.content.location);
rover.mode_guided.set_desired_location(cmd.content.location);
return true;
}
@ -927,11 +927,9 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
break;
}
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);
rover.nav_set_yaw_speed();
// send yaw change and target speed to guided mode controller
float target_speed = constrain_float(packet.param2 * rover.g.speed_cruise, -rover.g.speed_cruise, rover.g.speed_cruise);
rover.mode_guided.set_desired_heading_delta_and_speed(packet.param1, target_speed);
result = MAV_RESULT_ACCEPTED;
break;
}
@ -1044,8 +1042,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
if (rover.control_mode != &rover.mode_guided) {
break;
}
// record the time when the last message arrived
rover.guided_control.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) {
@ -1070,7 +1066,8 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
// 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);
// use body_yaw_rate field
rover.mode_guided.set_desired_turn_rate_and_speed((RAD_TO_DEG * packet.body_yaw_rate) * 100.0f, target_speed);
}
break;
}
@ -1098,9 +1095,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
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_control.msg_time_ms = AP_HAL::millis();
// prepare and send target position
Location target_loc = rover.current_loc;
if (!pos_ignore) {
@ -1128,7 +1122,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
}
}
float target_speed = 0.0f;
float target_steer_speed = 0.0f;
float target_turn_rate_cds = 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;
@ -1140,20 +1134,20 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
vely = velx * rover.ahrs.sin_yaw() + vely * rover.ahrs.cos_yaw();
}
target_speed = vely;
target_steer_speed = RAD_TO_DEG * packet.yaw_rate;
target_turn_rate_cds = 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);
rover.mode_guided.set_desired_location(target_loc);
if (!is_zero(target_speed)) {
rover.guided_control.target_speed = target_speed;
rover.mode_guided.set_desired_speed(target_speed);
}
} else if (pos_ignore && !vel_ignore && acc_ignore) {
rover.set_guided_velocity(target_steer_speed, target_speed);
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, target_speed);
} else if (!pos_ignore && vel_ignore && acc_ignore) {
rover.set_guided_WP(target_loc);
rover.mode_guided.set_desired_location(target_loc);
} else {
// result = MAV_RESULT_FAILED; // TODO : support that
}
@ -1181,9 +1175,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
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_control.msg_time_ms = AP_HAL::millis();
// prepare and send target position
Location target_loc = rover.current_loc;
if (!pos_ignore) {
@ -1196,24 +1187,24 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
target_loc.lng = packet.lon_int;
}
float target_speed = 0.0f;
float target_steer_speed = 0.0f;
float target_turn_rate_cds = 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;
target_turn_rate_cds = 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);
rover.mode_guided.set_desired_location(target_loc);
if (!is_zero(target_speed)) {
rover.guided_control.target_speed = target_speed;
rover.mode_guided.set_desired_speed(target_speed);
}
} else if (pos_ignore && !vel_ignore && acc_ignore) {
rover.set_guided_velocity(target_steer_speed, target_speed);
rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, target_speed);
} else if (!pos_ignore && vel_ignore && acc_ignore) {
rover.set_guided_WP(target_loc);
rover.mode_guided.set_desired_location(target_loc);
} else {
// result = MAV_RESULT_FAILED; // TODO : support that
}

14
APMrover2/Rover.h
View File

@ -395,14 +395,6 @@ private:
// time that rudder/steering arming has been running
uint32_t rudder_arm_timer;
// Store parameters from Guided msg
struct {
float turn_angle; // target heading in centi-degrees
float target_speed; // target speed in m/s
float target_steer_speed; // target steer speed in degree/s
uint32_t msg_time_ms; // time of last guided message
} guided_control;
// Store the time the last GPS message was received.
uint32_t last_gps_msg_ms{0};
@ -488,11 +480,9 @@ private:
void Log_Arm_Disarm();
void load_parameters(void);
bool use_pivot_steering(void);
bool use_pivot_steering(float yaw_error_cd);
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 update_home_from_EKF();
bool set_home_to_current_location(bool lock);
bool set_home(const Location& loc, bool lock);
@ -576,9 +566,7 @@ private:
bool motor_active();
void update_home();
void accel_cal_update(void);
void nav_set_yaw_speed();
bool do_yaw_rotation();
void nav_set_speed();
bool in_stationary_loiter(void);
void crash_check();
#if ADVANCED_FAILSAFE == ENABLED

8
APMrover2/Steering.cpp
View File

@ -3,7 +3,7 @@
/*
work out if we are going to use pivot steering
*/
bool Rover::use_pivot_steering(void)
bool Rover::use_pivot_steering(float yaw_error_cd)
{
// check cases where we clearly cannot use pivot steering
if (control_mode->is_autopilot_mode() || !g2.motors.have_skid_steering() || g.pivot_turn_angle <= 0) {
@ -12,16 +12,16 @@ bool Rover::use_pivot_steering(void)
}
// calc bearing error
const int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
const float yaw_error = yaw_error_cd / 100.0f;
// if error is larger than pivot_turn_angle start pivot steering
if (bearing_error > g.pivot_turn_angle) {
if (yaw_error > g.pivot_turn_angle) {
pivot_steering_active = true;
return true;
}
// if within 10 degrees of the target heading, exit pivot steering
if (bearing_error < 10) {
if (yaw_error < 10.0f) {
pivot_steering_active = false;
return false;
}

35
APMrover2/commands.cpp
View File

@ -27,38 +27,6 @@ void Rover::set_auto_WP(const struct Location& loc)
wp_distance = wp_totalDistance;
}
void Rover::set_guided_WP(const struct Location& loc)
{
rover.mode_guided.guided_mode = ModeGuided::Guided_WP;
// copy the current location into the OldWP slot
// ---------------------------------------
prev_WP = current_loc;
// Load the next_WP slot
// ---------------------
next_WP = loc;
rover.guided_control.target_speed = g.speed_cruise;
// this is handy for the groundstation
wp_totalDistance = get_distance(current_loc, next_WP);
wp_distance = wp_totalDistance;
rover.rtl_complete = false;
}
void Rover::set_guided_velocity(float target_steer_speed, float target_speed)
{
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;
// this is handy for the groundstation
wp_totalDistance = 0;
wp_distance = 0.0f;
rover.rtl_complete = false;
}
// checks if we should update ahrs home position from the EKF's position
void Rover::update_home_from_EKF()
{
@ -118,9 +86,6 @@ bool Rover::set_home(const Location& loc, bool lock)
// initialise navigation to home
next_WP = prev_WP = home;
// Load home for a default guided_WP
set_guided_WP(home);
}
// lock home position

47
APMrover2/commands_logic.cpp
View File

@ -360,53 +360,6 @@ bool Rover::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
return result;
}
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_control.msg_time_ms) > 3000) {
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);
return;
}
const int32_t steering = steerController.get_steering_out_angle_error(guided_control.turn_angle);
g2.motors.set_steering(steering);
// 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.
const float target_speed = g.speed_cruise * guided_control.target_speed * 2.0f;
rover.control_mode->calc_throttle(target_speed);
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()
{
// 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_control.msg_time_ms) > 3000) {
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);
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_control.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);
g2.motors.set_steering(steer_value);
rover.control_mode->calc_throttle(guided_control.target_speed);
Log_Write_GuidedTarget(rover.mode_guided.guided_mode, Vector3f(steer_value, 0.0f, 0.0f), Vector3f(guided_control.target_speed, 0.0f, 0.0f));
}
/********************************************************************************/
// Condition (May) commands
/********************************************************************************/