diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 1f90e5aa88..74ddbedac3 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -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; diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index d604908f83..84a330304c 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -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(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; } diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 11f8113437..de7250a82d 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -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); diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp index 58bd459a13..32024914ea 100644 --- a/APMrover2/Steering.cpp +++ b/APMrover2/Steering.cpp @@ -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); + } } } diff --git a/APMrover2/commands.cpp b/APMrover2/commands.cpp index 20b4f2581b..5458e81063 100644 --- a/APMrover2/commands.cpp +++ b/APMrover2/commands.cpp @@ -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() diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index 41db844d47..32a83739d5 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -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 /********************************************************************************/ diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 93a499e205..5d113eb87b 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -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