From 2d09db4ecb8fecbb04e1d50aeee1ec8cf28b6c9a Mon Sep 17 00:00:00 2001 From: Grant Morphett Date: Thu, 15 Sep 2016 22:09:45 +1000 Subject: [PATCH] Rover: added a new form of GUIDED mode Rover now accepts a new message MAV_CMD_NAV_SET_YAW_SPEED which has an angle in centidegrees and a speed scale and the rover will drive based on these inputs. --- APMrover2/APMrover2.cpp | 67 +++++++++++++++++++++++++----------- APMrover2/GCS_Mavlink.cpp | 23 ++++++++++++- APMrover2/Rover.h | 14 +++++++- APMrover2/commands_logic.cpp | 23 +++++++++++++ APMrover2/defines.h | 6 ++++ 5 files changed, 110 insertions(+), 23 deletions(-) diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 86519adc56..286973b0ad 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -412,21 +412,34 @@ void Rover::update_current_mode(void) calc_throttle(g.speed_cruise); break; - case GUIDED: - if (rtl_complete || verify_RTL()) { - // we have reached destination so stop where we are - if (channel_throttle->get_servo_out() != g.throttle_min.get()) { - gcs_send_mission_item_reached_message(0); + case GUIDED: { + switch (guided_mode){ + case Guided_Angle: + nav_set_yaw_speed(); + break; + + case Guided_WP: + if (rtl_complete || verify_RTL()) { + // we have reached destination so stop where we are + if (channel_throttle->get_servo_out() != g.throttle_min.get()) { + gcs_send_mission_item_reached_message(0); + } + channel_throttle->set_servo_out(g.throttle_min.get()); + channel_steer->set_servo_out(0); + lateral_acceleration = 0; + } else { + calc_lateral_acceleration(); + calc_nav_steer(); + calc_throttle(g.speed_cruise); } - channel_throttle->set_servo_out(g.throttle_min.get()); - channel_steer->set_servo_out(0); - lateral_acceleration = 0; - } else { - calc_lateral_acceleration(); - calc_nav_steer(); - calc_throttle(g.speed_cruise); + break; + + default: + gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode"); + break; } break; + } case STEERING: { /* @@ -509,17 +522,29 @@ void Rover::update_navigation() break; case GUIDED: - // no loitering around the wp with the rover, goes direct to the wp position - calc_lateral_acceleration(); - calc_nav_steer(); - if (rtl_complete || verify_RTL()) { - // we have reached destination so stop where we are - channel_throttle->set_servo_out(g.throttle_min.get()); - channel_steer->set_servo_out(0); - lateral_acceleration = 0; + switch (guided_mode){ + case Guided_Angle: + nav_set_yaw_speed(); + break; + + case Guided_WP: + // no loitering around the wp with the rover, goes direct to the wp position + calc_lateral_acceleration(); + calc_nav_steer(); + if (rtl_complete || verify_RTL()) { + // we have reached destination so stop where we are + channel_throttle->set_servo_out(g.throttle_min.get()); + channel_steer->set_servo_out(0); + lateral_acceleration = 0; + } + break; + + default: + gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode"); + break; } break; } } - + AP_HAL_MAIN_CALLBACKS(&rover); diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index d590c8626b..d712fca751 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -796,9 +796,11 @@ bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd) // only accept position updates when in GUIDED mode return false; } - rover.guided_WP = cmd.content.location; + // This method is only called when we are in Guided WP GUIDED mode + rover.guided_mode = Guided_WP; + // make any new wp uploaded instant (in case we are already in Guided mode) rover.rtl_complete = false; rover.set_guided_WP(); @@ -1166,6 +1168,25 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) result = rover.compass.handle_mag_cal_command(packet); break; + case MAV_CMD_NAV_SET_YAW_SPEED: + { + // param1 : yaw angle to adjust direction by in centidegress + // param2 : Speed - normalized to 0 .. 1 + + // exit if vehicle is not in Guided mode + if (rover.control_mode != GUIDED) { + break; + } + + 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.nav_set_yaw_speed(); + result = MAV_RESULT_ACCEPTED; + break; + } + default: break; } diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 6689bc8a68..155c74b01c 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -369,7 +369,18 @@ private: bool auto_throttle_mode; // Store the time the last GPS message was received. - uint32_t last_gps_msg_ms{0}; + 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; + + // Guided + GuidedMode guided_mode; // stores which GUIDED mode the vehicle is in + private: // private member functions @@ -520,6 +531,7 @@ private: bool motor_active(); void update_home(); void accel_cal_update(void); + void nav_set_yaw_speed(); public: bool print_log_menu(void); int8_t dump_log(uint8_t argc, const Menu::arg *argv); diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index f6fdd0a79f..572a813024 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -307,6 +307,29 @@ bool Rover::verify_loiter_unlim() return false; } +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) + { + gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping"); + channel_throttle->set_servo_out(g.throttle_min.get()); + channel_steer->set_servo_out(0); + lateral_acceleration = 0; + return; + } + + channel_steer->set_servo_out(steerController.get_steering_out_angle_error(guided_yaw_speed.turn_angle)); + + // 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; + calc_throttle(target_speed); + + return; +} + /********************************************************************************/ // Condition (May) commands /********************************************************************************/ diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 97b6cbdb57..c9a0f1ad00 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -40,6 +40,12 @@ enum mode { INITIALISING=16 }; +enum GuidedMode { + Guided_WP, + Guided_Angle +}; + + // types of failsafe events #define FAILSAFE_EVENT_THROTTLE (1<<0) #define FAILSAFE_EVENT_GCS (1<<1)