Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

ROVER: put all guided param in one structure

Browse Source
This commit is contained in:
Pierre Kancir 2017-02-15 20:11:08 +01:00 committed by Grant Morphett
parent 045d171ab9
commit 670e7b7914
5 changed files with 27 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
APMrover2/APMrover2.cpp
View File

@ -474,9 +474,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;

16
APMrover2/GCS_Mavlink.cpp
View File

@ -1083,9 +1083,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;
@ -1252,7 +1252,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
break;
}
// record the time when 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) {
@ -1306,7 +1306,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 when 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;
@ -1355,7 +1355,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);
@ -1391,7 +1391,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;
@ -1417,7 +1417,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);

16
APMrover2/Rover.h
View File

@ -391,19 +391,17 @@ private:
// 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();

6
APMrover2/commands.cpp
View File

@ -47,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;
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;
@ -58,8 +58,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.0f;

14
APMrover2/commands_logic.cpp
View File

@ -375,7 +375,7 @@ 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);
@ -383,13 +383,13 @@ void Rover::nav_set_yaw_speed()
return;
}
const int32_t steering = steerController.get_steering_out_angle_error(guided_yaw_speed.turn_angle);
const 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));
@ -399,7 +399,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);
@ -412,14 +412,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.0f, 0.0f), Vector3f(guided_target_speed, 0.0f, 0.0f));
Log_Write_GuidedTarget(guided_mode, Vector3f(steer_value, 0.0f, 0.0f), Vector3f(guided_control.target_speed, 0.0f, 0.0f));
}
/********************************************************************************/