Skip to content

Commit

Permalink
ROVER: put all guided param in one structure
Browse files Browse the repository at this point in the history
  • Loading branch information
@khancyr
khancyr committed Feb 16, 2017
1 parent 2eae499 commit 44ead7d
Show file tree
Hide file tree
Showing 5 changed files with 27 additions and 29 deletions.
4 changes: 2 additions & 2 deletions APMrover2/APMrover2.cpp
View file
Expand Up @@ -460,9 +460,9 @@ void Rover::update_current_mode(void)
} else {
calc_lateral_acceleration();
calc_nav_steer();
calc_throttle(guided_target_speed);
calc_throttle(rover.guided_control.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));
Vector3f(rover.guided_control.target_speed, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 0));
}
break;

Expand Down
16 changes: 8 additions & 8 deletions APMrover2/GCS_Mavlink.cpp
View file
Expand Up @@ -1084,9 +1084,9 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
}

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.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();
result = MAV_RESULT_ACCEPTED;
break;
Expand Down Expand Up @@ -1253,7 +1253,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
break;
}
// record the time where the last message arrived
rover.guided_yaw_speed.msg_time_ms = AP_HAL::millis();
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) {
Expand Down Expand Up @@ -1307,7 +1307,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
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();
rover.guided_control.msg_time_ms = AP_HAL::millis();

// prepare and send target position
Location target_loc = rover.current_loc;
Expand Down Expand Up @@ -1356,7 +1356,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
if (!pos_ignore && !vel_ignore && acc_ignore) {
rover.set_guided_WP(target_loc);
if (!is_zero(target_speed)) {
rover.guided_target_speed = target_speed;
rover.guided_control.target_speed = target_speed;
}
} else if (pos_ignore && !vel_ignore && acc_ignore) {
rover.set_guided_velocity(target_steer_speed, target_speed);
Expand Down Expand Up @@ -1390,7 +1390,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
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();
rover.guided_control.msg_time_ms = AP_HAL::millis();

// prepare and send target position
Location target_loc = rover.current_loc;
Expand All @@ -1416,7 +1416,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
if (!pos_ignore && !vel_ignore && acc_ignore) {
rover.set_guided_WP(target_loc);
if (!is_zero(target_speed)) {
rover.guided_target_speed = target_speed;
rover.guided_control.target_speed = target_speed;
}
} else if (pos_ignore && !vel_ignore && acc_ignore) {
rover.set_guided_velocity(target_steer_speed, target_speed);
Expand Down
16 changes: 7 additions & 9 deletions APMrover2/Rover.h
View file
Expand Up @@ -385,19 +385,17 @@ class Rover : public AP_HAL::HAL::Callbacks {

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

// Store parameters from NAV_SET_YAW_SPEED
struct {
float turn_angle;
float target_speed;
uint32_t msg_time_ms;
} guided_yaw_speed;

private:
// private member functions
void ahrs_update();
Expand Down
6 changes: 3 additions & 3 deletions APMrover2/commands.cpp
View file
Expand Up @@ -37,7 +37,7 @@ void Rover::set_guided_WP(const struct Location& loc)
// Load the next_WP slot
// ---------------------
next_WP = loc;
guided_target_speed = g.speed_cruise;
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;
Expand All @@ -48,8 +48,8 @@ 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;
guided_target_steer_speed = target_steer_speed;
guided_target_speed = target_speed;
rover.guided_control.target_steer_speed = target_steer_speed;
rover.guided_control.target_speed = target_speed;

next_WP = current_loc;
lateral_acceleration = 0;
Expand Down
14 changes: 7 additions & 7 deletions APMrover2/commands_logic.cpp
View file
Expand Up @@ -368,21 +368,21 @@ bool Rover::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
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) {
if ((millis() - guided_control.msg_time_ms) > 3000) {
gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED 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;
return;
}

int32_t steering = steerController.get_steering_out_angle_error(guided_yaw_speed.turn_angle);
int32_t steering = steerController.get_steering_out_angle_error(guided_control.turn_angle);
SRV_Channels::set_output_scaled(SRV_Channel::k_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.
float target_speed = g.speed_cruise * guided_yaw_speed.target_speed * 2;
float target_speed = g.speed_cruise * guided_control.target_speed * 2;
calc_throttle(target_speed);

Log_Write_GuidedTarget(guided_mode, Vector3f(steering, 0, 0), Vector3f(target_speed, 0, 0));
Expand All @@ -392,7 +392,7 @@ void Rover::nav_set_yaw_speed()
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) {
if ((millis() - guided_control.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);
Expand All @@ -405,14 +405,14 @@ void Rover::nav_set_speed()
prev_WP = current_loc;
next_WP = current_loc;

const int32_t steer_value = steerController.get_steering_out_rate(guided_target_steer_speed);
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);

SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steer_value);
calc_throttle(guided_target_speed);
calc_throttle(guided_control.target_speed);

Log_Write_GuidedTarget(guided_mode, Vector3f(guided_target_steer_speed, 0, 0), Vector3f(guided_target_speed, 0, 0));
Log_Write_GuidedTarget(guided_mode, Vector3f(steer_value, 0, 0), Vector3f(guided_control.target_speed, 0, 0));
}

/********************************************************************************/
Expand Down

0 comments on commit 44ead7d

Please sign in to comment.