diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 98f5a07ab0..dc2ef39326 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -68,7 +68,7 @@ * ..and many more. * * Code commit statistics can be found here: https://github.com/diydrones/ardupilot/graphs/contributors - * Wiki: http://copter.ardupilot.com/ + * Wiki: http://copter.ardupilot.org/ * Requires modified version of Arduino, which can be found here: http://ardupilot.com/downloads/?category=6 * */ @@ -129,6 +129,7 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = { #if FRSKY_TELEM_ENABLED == ENABLED SCHED_TASK(frsky_telemetry_send, 5, 75), #endif + SCHED_TASK(terrain_update, 10, 100), #if EPM_ENABLED == ENABLED SCHED_TASK(epm_update, 10, 75), #endif @@ -303,9 +304,6 @@ void Sub::rc_loop() // --------------------------- void Sub::throttle_loop() { - // get altitude and climb rate from inertial lib - read_inertial_altitude(); - // update throttle_low_comp value (controls priority of throttle vs attitude control) update_throttle_thr_mix(); @@ -457,6 +455,9 @@ void Sub::three_hz_loop() // check if we've lost contact with the ground station failsafe_gcs_check(); + // check if we've lost terrain data + failsafe_terrain_check(); + #if AC_FENCE == ENABLED // check if we have breached a fence fence_check(); @@ -503,24 +504,14 @@ void Sub::one_hz_loop() check_usb_mux(); -#if AP_TERRAIN_AVAILABLE && AC_TERRAIN - terrain.update(); - - // tell the rangefinder our height, so it can go into power saving - // mode if available -#if CONFIG_SONAR == ENABLED - float height; - if (terrain.height_above_terrain(height, true)) { - sonar.set_estimated_terrain_height(height); - } -#endif -#endif - // update position controller alt limits update_poscon_alt_max(); // enable/disable raw gyro/accel logging ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW)); + + // log terrain data + terrain_logging(); } // called at 50hz diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 079729ab84..0aba37ca71 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -33,7 +33,7 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan) uint32_t custom_mode = control_mode; // set system as critical if any failsafe have triggered - if (failsafe.radio || failsafe.battery || failsafe.gcs || failsafe.ekf) { + if (failsafe.radio || failsafe.battery || failsafe.gcs || failsafe.ekf || failsafe.terrain) { system_status = MAV_STATE_CRITICAL; } @@ -715,6 +715,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) case MSG_FENCE_STATUS: case MSG_WIND: + case MSG_POSITION_TARGET_GLOBAL_INT: // unused break; @@ -973,9 +974,9 @@ GCS_MAVLINK::data_stream_send(void) } -void GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd) +bool GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd) { - sub.do_guided(cmd); + return sub.do_guided(cmd); } void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd) @@ -1064,8 +1065,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } + case MAVLINK_MSG_ID_MISSION_ITEM_INT: + { + if (handle_mission_item(msg, sub.mission)) { + sub.DataFlash.Log_Write_EntireMission(sub.mission); + } + break; + } + // read an individual command from EEPROM and send it to the GCS - case MAVLINK_MSG_ID_MISSION_REQUEST: // MAV ID: 40 + case MAVLINK_MSG_ID_MISSION_REQUEST_INT: + case MAVLINK_MSG_ID_MISSION_REQUEST: // MAV ID: 40, 51 { handle_mission_request(sub.mission, msg); break; diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 46ffcce0e0..d339005c9e 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -306,6 +306,7 @@ struct PACKED log_Control_Tuning { int32_t baro_alt; int16_t desired_sonar_alt; int16_t sonar_alt; + float terr_alt; int16_t desired_climb_rate; int16_t climb_rate; }; @@ -313,6 +314,12 @@ struct PACKED log_Control_Tuning { // Write a control tuning packet void Sub::Log_Write_Control_Tuning() { + // get terrain altitude + float terr_alt = 0.0f; +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + terrain.height_above_terrain(terr_alt, true); +#endif + struct log_Control_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG), time_us : AP_HAL::micros64(), @@ -324,6 +331,7 @@ void Sub::Log_Write_Control_Tuning() baro_alt : baro_alt, desired_sonar_alt : (int16_t)target_sonar_alt, sonar_alt : sonar_alt, + terr_alt : terr_alt, desired_climb_rate : (int16_t)pos_control.get_vel_target_z(), climb_rate : climb_rate }; @@ -655,6 +663,36 @@ void Sub::Log_Write_Precland() #endif // PRECISION_LANDING == ENABLED } +// precision landing logging +struct PACKED log_GuidedTarget { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t type; + float pos_target_x; + float pos_target_y; + float pos_target_z; + float vel_target_x; + float vel_target_y; + float vel_target_z; +}; + +// Write a Guided mode target +void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) +{ + struct log_GuidedTarget pkt = { + LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG), + time_us : AP_HAL::micros64(), + type : target_type, + pos_target_x : pos_target.x, + pos_target_y : pos_target.y, + pos_target_z : pos_target.z, + vel_target_x : vel_target.x, + vel_target_y : vel_target.y, + vel_target_z : vel_target.z + }; + DataFlash.WriteBlock(&pkt, sizeof(pkt)); +} + const struct LogStructure Sub::log_structure[] = { LOG_COMMON_STRUCTURES, #if AUTOTUNE_ENABLED == ENABLED @@ -670,7 +708,7 @@ const struct LogStructure Sub::log_structure[] = { { LOG_NAV_TUNING_MSG, sizeof(log_Nav_Tuning), "NTUN", "Qffffffffff", "TimeUS,DPosX,DPosY,PosX,PosY,DVelX,DVelY,VelX,VelY,DAccX,DAccY" }, { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), - "CTUN", "Qfffffecchh", "TimeUS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" }, + "CTUN", "Qfffffeccfhh", "TimeUS,ThrIn,ABst,ThrOut,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt" }, { LOG_PERFORMANCE_MSG, sizeof(log_Performance), "PM", "QHHIhBHI", "TimeUS,NLon,NLoop,MaxT,PMT,I2CErr,INSErr,LogDrop" }, { LOG_MOTBATT_MSG, sizeof(log_MotBatt), @@ -693,6 +731,8 @@ const struct LogStructure Sub::log_structure[] = { "HELI", "Qff", "TimeUS,DRRPM,ERRPM" }, { LOG_PRECLAND_MSG, sizeof(log_Precland), "PL", "QBffffff", "TimeUS,Heal,bX,bY,eX,eY,pX,pY" }, + { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget), + "GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" }, }; #if CLI_ENABLED == ENABLED @@ -785,6 +825,7 @@ void Sub::Log_Write_Baro(void) {} void Sub::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) {} void Sub::Log_Write_Home_And_Origin() {} void Sub::Log_Sensor_Health() {} +void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}; #if OPTFLOW == ENABLED void Sub::Log_Write_Optflow() {} diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 29bd64caf7..a9d2bae890 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -138,6 +138,15 @@ const AP_Param::Info Sub::var_info[] = { // @User: Standard GSCALAR(rtl_altitude, "RTL_ALT", RTL_ALT), + // @Param: RTL_CONE_SLOPE + // @DisplayName: RTL cone slope + // @Description: Defines a cone above home which determines maximum climb + // @Range: 0.5 10.0 + // @Increment: .1 + // @Values: 0:Disabled,1:Shallow,3:Steep + // @User: Standard + GSCALAR(rtl_cone_slope, "RTL_CONE_SLOPE", RTL_CONE_SLOPE), + // @Param: RTL_SPEED // @DisplayName: RTL speed // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally while flying home. If this is set to zero, WPNAV_SPEED will be used instead. @@ -1023,6 +1032,13 @@ const AP_Param::Info Sub::var_info[] = { // @User: Standard GSCALAR(throw_motor_start, "THROW_MOT_START", 0), + // @Param: TERRAIN_FOLLOW + // @DisplayName: Terrain Following use control + // @Description: This enables terrain following for RTL and LAND flight modes. To use this option TERRAIN_ENABLE must be 1 and the GCS must support sending terrain data to the aircraft. In RTL the RTL_ALT will be considered a height above the terrain. In LAND mode the vehicle will slow to LAND_SPEED 10m above terrain (instead of 10m above home). This parameter does not affect AUTO and Guided which use a per-command flag to determine if the height is above-home, absolute or above-terrain. + // @Values: 0:Do Not Use in RTL and Land 1:Use in RTL and Land + // @User: Standard + GSCALAR(terrain_follow, "TERRAIN_FOLLOW", 0), + AP_VAREND }; diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index eb194554b0..85eb8150a7 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -180,6 +180,7 @@ public: k_param_disarm_delay, k_param_fs_crash_check, k_param_throw_motor_start, + k_param_terrain_follow, // 94 // 97: RSSI k_param_rssi = 97, @@ -217,7 +218,8 @@ public: // 135 : reserved for Solo until features merged with master // k_param_rtl_speed_cms = 135, - k_param_fs_batt_curr_rtl, // 136 + k_param_fs_batt_curr_rtl, + k_param_rtl_cone_slope, // 137 // // 140: Sensor parameters @@ -403,6 +405,7 @@ public: AP_Int16 rtl_altitude; AP_Int16 rtl_speed_cms; + AP_Float rtl_cone_slope; AP_Float sonar_gain; AP_Int8 failsafe_battery_enabled; // battery failsafe enabled @@ -473,6 +476,7 @@ public: AP_Int16 gcs_pid_mask; AP_Int8 throw_motor_start; + AP_Int8 terrain_follow; // RC channels RC_Channel rc_1; diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index e4c60cc8d3..3ebc69850b 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -39,7 +39,6 @@ Sub::Sub(void) : guided_mode(Guided_TakeOff), rtl_state(RTL_InitialClimb), rtl_state_complete(false), - rtl_alt(0.0f), circle_pilot_yaw_override(false), simple_cos_yaw(1.0f), simple_sin_yaw(0.0f), diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 68cd925348..365c047ecb 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -35,6 +35,7 @@ // Common dependencies #include +#include #include #include #include @@ -284,10 +285,13 @@ private: uint8_t battery : 1; // 2 // A status flag for the battery failsafe uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe uint8_t ekf : 1; // 5 // true if ekf failsafe has occurred + uint8_t terrain : 1; // 6 // true if the missing terrain data failsafe has occurred int8_t radio_counter; // number of iterations with throttle below throttle_fs_value uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe + uint32_t terrain_first_failure_ms; // the first time terrain data access failed - used to calculate the duration of the failure + uint32_t terrain_last_failure_ms; // the most recent time terrain data access failed } failsafe; // sensor health for logging @@ -351,7 +355,16 @@ private: // RTL RTLState rtl_state; // records state of rtl (initial climb, returning home, etc) bool rtl_state_complete; // set to true if the current state is completed - float rtl_alt; // altitude the vehicle is returning at + + struct { + // NEU w/ Z element alt-above-ekf-origin unless use_terrain is true in which case Z element is alt-above-terrain + Location_Class origin_point; + Location_Class climb_target; + Location_Class return_target; + Location_Class descent_target; + bool land; + bool terrain_used; + } rtl_path; // Circle bool circle_pilot_yaw_override; // true if pilot is overriding yaw @@ -409,7 +422,7 @@ private: // 3D Location vectors // Current location of the Sub (altitude is relative to home) - struct Location current_loc; + Location_Class current_loc; // Navigation Yaw control // auto flight mode's yaw mode @@ -645,6 +658,7 @@ private: void Log_Write_Home_And_Origin(); void Log_Sensor_Health(); void Log_Write_Precland(); + void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); void Log_Write_Vehicle_Startup_Messages(); void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page); void start_logging() ; @@ -672,7 +686,6 @@ private: bool verify_loiter_time(); bool verify_RTL(); bool verify_wait_delay(); - bool verify_change_alt(); bool verify_within_distance(); bool verify_yaw(); void do_take_picture(); @@ -686,9 +699,10 @@ private: void althold_run(); bool auto_init(bool ignore_checks); void auto_run(); - void auto_takeoff_start(float final_alt_above_home); + void auto_takeoff_start(const Location& dest_loc); void auto_takeoff_run(); void auto_wp_start(const Vector3f& destination); + void auto_wp_start(const Location_Class& dest_loc); void auto_wp_run(); void auto_spline_run(); void auto_land_start(); @@ -696,7 +710,7 @@ private: void auto_land_run(); void auto_rtl_start(); void auto_rtl_run(); - void auto_circle_movetoedge_start(); + void auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m); void auto_circle_start(); void auto_circle_run(); void auto_nav_guided_start(); @@ -740,12 +754,13 @@ private: bool flip_init(bool ignore_checks); void flip_run(); bool guided_init(bool ignore_checks); - void guided_takeoff_start(float final_alt_above_home); + bool guided_takeoff_start(float final_alt_above_home); void guided_pos_control_start(); void guided_vel_control_start(); void guided_posvel_control_start(); void guided_angle_control_start(); void guided_set_destination(const Vector3f& destination); + bool guided_set_destination(const Location_Class& dest_loc); void guided_set_velocity(const Vector3f& velocity); void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity); void guided_set_angle(const Quaternion &q, float climb_rate_cms); @@ -788,6 +803,7 @@ private: bool throw_height_good(); bool rtl_init(bool ignore_checks); + void rtl_restart_without_terrain(); void rtl_run(); void rtl_climb_start(); void rtl_return_start(); @@ -798,6 +814,8 @@ private: void rtl_descent_run(); void rtl_land_start(); void rtl_land_run(); + void rtl_build_path(bool terrain_following_allowed); + void rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target, bool terrain_following_allowed); float get_RTL_alt(); bool sport_init(bool ignore_checks); void sport_run(); @@ -822,6 +840,9 @@ private: void failsafe_battery_event(void); void failsafe_gcs_check(); void failsafe_gcs_off_event(void); + void failsafe_terrain_check(); + void failsafe_terrain_set_status(bool data_ok); + void failsafe_terrain_on_event(); void set_mode_RTL_or_land_with_pause(mode_reason_t reason); void update_events(); void failsafe_enable(); @@ -864,6 +885,7 @@ private: void pre_arm_rc_checks(); bool pre_arm_gps_checks(bool display_failure); bool pre_arm_ekf_attitude_check(); + bool pre_arm_terrain_check(); bool arm_checks(bool display_failure, bool arming_from_gcs); void init_disarm_motors(); void motors_output(); @@ -912,6 +934,9 @@ private: void read_battery(void); void read_receiver_rssi(void); void epm_update(); + void terrain_update(); + void terrain_logging(); + bool terrain_use(); void report_batt_monitor(); void report_frame(); void report_radio(); @@ -974,7 +999,6 @@ private: #endif void do_wait_delay(const AP_Mission::Mission_Command& cmd); void do_within_distance(const AP_Mission::Mission_Command& cmd); - void do_change_alt(const AP_Mission::Mission_Command& cmd); void do_yaw(const AP_Mission::Mission_Command& cmd); void do_change_speed(const AP_Mission::Mission_Command& cmd); void do_set_home(const AP_Mission::Mission_Command& cmd); @@ -996,8 +1020,7 @@ private: #if NAV_GUIDED == ENABLED bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd); #endif - void auto_spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_spline_destination); - + void auto_spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination); void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode); void log_init(void); void run_cli(AP_HAL::UARTDriver *port); diff --git a/ArduSub/arming_checks.cpp b/ArduSub/arming_checks.cpp index 2ffbe1236d..d2c2dae035 100644 --- a/ArduSub/arming_checks.cpp +++ b/ArduSub/arming_checks.cpp @@ -318,6 +318,12 @@ bool Sub::pre_arm_checks(bool display_failure) return false; } #endif + + // check for missing terrain data + if (!pre_arm_terrain_check()) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Terrain data"); + return false; + } } // check throttle is above failsafe throttle @@ -468,6 +474,24 @@ bool Sub::pre_arm_ekf_attitude_check() return filt_status.flags.attitude; } +// check we have required terrain data +bool Sub::pre_arm_terrain_check() +{ +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + // succeed if not using terrain data + if (!terrain_use()) { + return true; + } + + // show terrain statistics + uint16_t terr_pending, terr_loaded; + terrain.get_statistics(terr_pending, terr_loaded); + return (terr_pending <= 0); +#else + return true; +#endif +} + // arm_checks - perform final checks before arming // always called just before arming. Return true if ok to arm // has side-effect that logging is started @@ -611,6 +635,14 @@ bool Sub::arm_checks(bool display_failure, bool arming_from_gcs) } } + // check for missing terrain data + if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) { + if (!pre_arm_terrain_check()) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Waiting for Terrain data"); + return false; + } + } + // check throttle if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) { // check throttle is not too low - must be above failsafe throttle diff --git a/ArduSub/capabilities.cpp b/ArduSub/capabilities.cpp index 026ac88f30..1b2e1b590a 100644 --- a/ArduSub/capabilities.cpp +++ b/ArduSub/capabilities.cpp @@ -6,8 +6,10 @@ void Sub::init_capabilities(void) { hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT); hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT); + hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_INT); hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED); hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT); hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION); hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET); + hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_TERRAIN); } diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 03b0b054df..876f575fbf 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -64,10 +64,6 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd) do_within_distance(cmd); break; - case MAV_CMD_CONDITION_CHANGE_ALT: // 113 - do_change_alt(cmd); - break; - case MAV_CMD_CONDITION_YAW: // 115 do_yaw(cmd); break; @@ -225,9 +221,6 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_CONDITION_DISTANCE: return verify_within_distance(); - case MAV_CMD_CONDITION_CHANGE_ALT: - return verify_change_alt(); - case MAV_CMD_CONDITION_YAW: return verify_yaw(); @@ -284,25 +277,20 @@ void Sub::do_RTL(void) void Sub::do_takeoff(const AP_Mission::Mission_Command& cmd) { // Set wp navigation target to safe altitude above current position - float takeoff_alt = cmd.content.location.alt; - takeoff_alt = MAX(takeoff_alt,current_loc.alt); - takeoff_alt = MAX(takeoff_alt,100.0f); - auto_takeoff_start(takeoff_alt); + auto_takeoff_start(cmd.content.location); } // do_nav_wp - initiate move to next waypoint void Sub::do_nav_wp(const AP_Mission::Mission_Command& cmd) { - const Vector3f &curr_pos = inertial_nav.get_position(); - const Vector3f local_pos = pv_location_to_vector_with_default(cmd.content.location, curr_pos); - // this will be used to remember the time in millis after we reach or pass the WP. loiter_time = 0; // this is the delay, stored in seconds - loiter_time_max = abs(cmd.p1); + loiter_time_max = cmd.p1; // Set wp navigation target - auto_wp_start(local_pos); + auto_wp_start(Location_Class(cmd.content.location)); + // if no delay set the waypoint as "fast" if (loiter_time_max == 0 ) { wp_nav.set_fast_waypoint(true); @@ -320,9 +308,21 @@ void Sub::do_land(const AP_Mission::Mission_Command& cmd) land_state = LAND_STATE_FLY_TO_LOCATION; // calculate and set desired location above landing target - Vector3f pos = pv_location_to_vector(cmd.content.location); - pos.z = inertial_nav.get_altitude(); - auto_wp_start(pos); + // convert to location class + Location_Class target_loc(cmd.content.location); + + // decide if we will use terrain following + int32_t curr_terr_alt_cm, target_terr_alt_cm; + if (current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) && + target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) { + curr_terr_alt_cm = MAX(curr_terr_alt_cm,200); + // if using terrain, set target altitude to current altitude above terrain + target_loc.set_alt_cm(curr_terr_alt_cm, Location_Class::ALT_FRAME_ABOVE_TERRAIN); + } else { + // set target altitude to current altitude above home + target_loc.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME); + } + auto_wp_start(target_loc); }else{ // set landing state land_state = LAND_STATE_DESCENDING; @@ -336,95 +336,72 @@ void Sub::do_land(const AP_Mission::Mission_Command& cmd) // note: caller should set yaw_mode void Sub::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) { - Vector3f target_pos; - - // get current position - Vector3f curr_pos = inertial_nav.get_position(); - - // default to use position provided - target_pos = pv_location_to_vector(cmd.content.location); + // convert back to location + Location_Class target_loc(cmd.content.location); // use current location if not provided - if(cmd.content.location.lat == 0 && cmd.content.location.lng == 0) { - wp_nav.get_wp_stopping_point_xy(target_pos); + if (target_loc.lat == 0 && target_loc.lng == 0) { + // To-Do: make this simpler + Vector3f temp_pos; + wp_nav.get_wp_stopping_point_xy(temp_pos); + target_loc.offset(temp_pos.x * 100.0f, temp_pos.y * 100.0f); } // use current altitude if not provided // To-Do: use z-axis stopping point instead of current alt - if( cmd.content.location.alt == 0 ) { - target_pos.z = curr_pos.z; + if (target_loc.alt == 0) { + // set to current altitude but in command's alt frame + int32_t curr_alt; + if (current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) { + target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame()); + } else { + // default to current altitude as alt-above-home + target_loc.set_alt_cm(current_loc.alt, current_loc.get_alt_frame()); + } } // start way point navigator and provide it the desired location - auto_wp_start(target_pos); + auto_wp_start(target_loc); } // do_circle - initiate moving in a circle void Sub::do_circle(const AP_Mission::Mission_Command& cmd) { - Vector3f curr_pos = inertial_nav.get_position(); - Vector3f circle_center = pv_location_to_vector(cmd.content.location); + Location_Class circle_center(cmd.content.location); + + // default lat/lon to current position if not provided + // To-Do: use stopping point or position_controller's target instead of current location to avoid jerk? + if (circle_center.lat == 0 && circle_center.lng == 0) { + circle_center.lat = current_loc.lat; + circle_center.lng = current_loc.lng; + } + + // default target altitude to current altitude if not provided + if (circle_center.alt == 0) { + int32_t curr_alt; + if (current_loc.get_alt_cm(circle_center.get_alt_frame(),curr_alt)) { + // circle altitude uses frame from command + circle_center.set_alt_cm(curr_alt,circle_center.get_alt_frame()); + } else { + // default to current altitude above origin + circle_center.set_alt_cm(current_loc.alt, current_loc.get_alt_frame()); + Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); + } + } + + // calculate radius uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1 - bool move_to_edge_required = false; - // set target altitude if not provided - if (cmd.content.location.alt == 0) { - circle_center.z = curr_pos.z; - } else { - move_to_edge_required = true; - } - - // set lat/lon position if not provided - // To-Do: use previous command's destination if it was a straight line or spline waypoint command - if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) { - circle_center.x = curr_pos.x; - circle_center.y = curr_pos.y; - } else { - move_to_edge_required = true; - } - - // set circle controller's center - circle_nav.set_center(circle_center); - - // set circle radius - if (circle_radius_m != 0) { - circle_nav.set_radius((float)circle_radius_m * 100.0f); - } - - // check if we need to move to edge of circle - if (move_to_edge_required) { - // move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge - auto_circle_movetoedge_start(); - } else { - // start circling - auto_circle_start(); - } + // move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge + auto_circle_movetoedge_start(circle_center, circle_radius_m); } // do_loiter_time - initiate loitering at a point for a given time period // note: caller should set yaw_mode void Sub::do_loiter_time(const AP_Mission::Mission_Command& cmd) { - Vector3f target_pos; - - // get current position - Vector3f curr_pos = inertial_nav.get_position(); - - // default to use position provided - target_pos = pv_location_to_vector(cmd.content.location); - - // use current location if not provided - if(cmd.content.location.lat == 0 && cmd.content.location.lng == 0) { - wp_nav.get_wp_stopping_point_xy(target_pos); - } - - // use current altitude if not provided - if( cmd.content.location.alt == 0 ) { - target_pos.z = curr_pos.z; - } - - // start way point navigator and provide it the desired location - auto_wp_start(target_pos); + // re-use loiter unlimited + do_loiter_unlimited(cmd); // setup loiter timer loiter_time = 0; @@ -434,19 +411,33 @@ void Sub::do_loiter_time(const AP_Mission::Mission_Command& cmd) // do_spline_wp - initiate move to next waypoint void Sub::do_spline_wp(const AP_Mission::Mission_Command& cmd) { - const Vector3f& curr_pos = inertial_nav.get_position(); - Vector3f local_pos = pv_location_to_vector_with_default(cmd.content.location, curr_pos); + Location_Class target_loc(cmd.content.location); + // use current lat, lon if zero + if (target_loc.lat == 0 && target_loc.lng == 0) { + target_loc.lat = current_loc.lat; + target_loc.lng = current_loc.lng; + } + // use current altitude if not provided + if (target_loc.alt == 0) { + // set to current altitude but in command's alt frame + int32_t curr_alt; + if (current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) { + target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame()); + } else { + // default to current altitude as alt-above-home + target_loc.set_alt_cm(current_loc.alt, current_loc.get_alt_frame()); + } + } // this will be used to remember the time in millis after we reach or pass the WP. loiter_time = 0; // this is the delay, stored in seconds - loiter_time_max = abs(cmd.p1); + loiter_time_max = cmd.p1; // determine segment start and end type bool stopped_at_start = true; AC_WPNav::spline_segment_end_type seg_end_type = AC_WPNav::SEGMENT_END_STOP; AP_Mission::Mission_Command temp_cmd; - Vector3f next_destination; // end of next segment // if previous command was a wp_nav command with no delay set stopped_at_start to false // To-Do: move processing of delay into wp-nav controller to allow it to determine the stopped_at_start value itself? @@ -460,19 +451,34 @@ void Sub::do_spline_wp(const AP_Mission::Mission_Command& cmd) } // if there is no delay at the end of this segment get next nav command + Location_Class next_loc; if (cmd.p1 == 0 && mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) { + next_loc = temp_cmd.content.location; + // default lat, lon to first waypoint's lat, lon + if (next_loc.lat == 0 && next_loc.lng == 0) { + next_loc.lat = target_loc.lat; + next_loc.lng = target_loc.lng; + } + // default alt to first waypoint's alt but in next waypoint's alt frame + if (next_loc.alt == 0) { + int32_t next_alt; + if (target_loc.get_alt_cm(next_loc.get_alt_frame(), next_alt)) { + next_loc.set_alt_cm(next_alt, next_loc.get_alt_frame()); + } else { + // default to first waypoints altitude + next_loc.set_alt_cm(target_loc.alt, target_loc.get_alt_frame()); + } + } // if the next nav command is a waypoint set end type to spline or straight if (temp_cmd.id == MAV_CMD_NAV_WAYPOINT) { seg_end_type = AC_WPNav::SEGMENT_END_STRAIGHT; - next_destination = pv_location_to_vector_with_default(temp_cmd.content.location, local_pos); }else if (temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) { seg_end_type = AC_WPNav::SEGMENT_END_SPLINE; - next_destination = pv_location_to_vector_with_default(temp_cmd.content.location, local_pos); } } // set spline navigation target - auto_spline_start(local_pos, stopped_at_start, seg_end_type, next_destination); + auto_spline_start(target_loc, stopped_at_start, seg_end_type, next_loc); } #if NAV_GUIDED == ENABLED @@ -724,11 +730,6 @@ void Sub::do_wait_delay(const AP_Mission::Mission_Command& cmd) condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds } -void Sub::do_change_alt(const AP_Mission::Mission_Command& cmd) -{ - // To-Do: store desired altitude in a variable so that it can be verified later -} - void Sub::do_within_distance(const AP_Mission::Mission_Command& cmd) { condition_value = cmd.content.distance.meters * 100; @@ -757,12 +758,6 @@ bool Sub::verify_wait_delay() return false; } -bool Sub::verify_change_alt() -{ - // To-Do: use recorded target altitude to verify we have reached the target - return true; -} - bool Sub::verify_within_distance() { // update distance calculation @@ -797,8 +792,6 @@ bool Sub::verify_yaw() // do_guided - start guided mode bool Sub::do_guided(const AP_Mission::Mission_Command& cmd) { - Vector3f pos_or_vel; // target location or velocity - // only process guided waypoint if we are in guided mode if (control_mode != GUIDED && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) { return false; @@ -808,11 +801,12 @@ bool Sub::do_guided(const AP_Mission::Mission_Command& cmd) switch (cmd.id) { case MAV_CMD_NAV_WAYPOINT: + { // set wp_nav's destination - pos_or_vel = pv_location_to_vector(cmd.content.location); - guided_set_destination(pos_or_vel); - return true; + Location_Class dest(cmd.content.location); + return guided_set_destination(dest); break; + } case MAV_CMD_CONDITION_YAW: do_yaw(cmd); diff --git a/ArduSub/compassmot.cpp b/ArduSub/compassmot.cpp index 96c28b4791..6eb4a2bbf8 100644 --- a/ArduSub/compassmot.cpp +++ b/ArduSub/compassmot.cpp @@ -31,6 +31,11 @@ uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan) ap.compass_mot = true; } + // initialise output + for (uint8_t i=0; i circle_nav.get_radius() && dist_to_center > 500) { - set_auto_yaw_mode(get_default_auto_yaw_mode(false)); - } else { - // vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle - set_auto_yaw_mode(AUTO_YAW_HOLD); - } + // if more than 3m then fly to edge + if (dist_to_edge > 300.0f) { + // set the state to move to the edge of the circle + auto_mode = Auto_CircleMoveToEdge; + + // convert circle_edge_neu to Location_Class + Location_Class circle_edge(circle_edge_neu); + + // convert altitude to same as command + circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame()); + + // initialise wpnav to move to edge of circle + if (!wp_nav.set_wp_destination(circle_edge)) { + // failure to set destination can only be because of missing terrain data + failsafe_terrain_on_event(); + } + + // if we are outside the circle, point at the edge, otherwise hold yaw + const Vector3f &curr_pos = inertial_nav.get_position(); + float dist_to_center = pythagorous2(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y); + if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) { + set_auto_yaw_mode(get_default_auto_yaw_mode(false)); + } else { + // vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle + set_auto_yaw_mode(AUTO_YAW_HOLD); + } + } else { + auto_circle_start(); + } } // auto_circle_start - initialises controller to fly a circle in AUTO flight mode +// assumes that circle_nav object has already been initialised with circle center and radius void Sub::auto_circle_start() { auto_mode = Auto_Circle; // initialise circle controller - // center was set in do_circle so initialise with current center circle_nav.init(circle_nav.get_center()); } @@ -501,8 +578,9 @@ void Sub::auto_loiter_run() // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - // run waypoint and z-axis postion controller - wp_nav.update_wpnav(); + // run waypoint and z-axis position controller + failsafe_terrain_set_status(wp_nav.update_wpnav()); + pos_control.update_z_controller(); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); } diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index d4dad9f060..ddb482f460 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -51,41 +51,51 @@ bool Sub::guided_init(bool ignore_checks) // guided_takeoff_start - initialises waypoint controller to implement take-off -void Sub::guided_takeoff_start(float final_alt_above_home) +bool Sub::guided_takeoff_start(float final_alt_above_home) { - guided_mode = Guided_TakeOff; - - // initialise wpnav destination - Vector3f target_pos = inertial_nav.get_position(); - target_pos.z = pv_alt_above_origin(final_alt_above_home); - wp_nav.set_wp_destination(target_pos); + guided_mode = Guided_TakeOff; - // initialise yaw - set_auto_yaw_mode(AUTO_YAW_HOLD); + // initialise wpnav destination + Location_Class target_loc = current_loc; + target_loc.set_alt_cm(final_alt_above_home, Location_Class::ALT_FRAME_ABOVE_HOME); - // clear i term when we're taking off - set_throttle_takeoff(); + if (!wp_nav.set_wp_destination(target_loc)) { + // failure to set destination can only be because of missing terrain data + Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); + // failure is propagated to GCS with NAK + return false; + } + + // initialise yaw + set_auto_yaw_mode(AUTO_YAW_HOLD); + + // clear i term when we're taking off + set_throttle_takeoff(); + + return true; } // initialise guided mode's position controller void Sub::guided_pos_control_start() { - // set to position control mode - guided_mode = Guided_WP; + // set to position control mode + guided_mode = Guided_WP; - // initialise waypoint and spline controller - wp_nav.wp_and_spline_init(); + // initialise waypoint and spline controller + wp_nav.wp_and_spline_init(); - // initialise wpnav to stopping point at current altitude - // To-Do: set to current location if disarmed? - // To-Do: set to stopping point altitude? - Vector3f stopping_point; - stopping_point.z = inertial_nav.get_altitude(); - wp_nav.get_wp_stopping_point_xy(stopping_point); - wp_nav.set_wp_destination(stopping_point); + // initialise wpnav to stopping point at current altitude + // To-Do: set to current location if disarmed? + // To-Do: set to stopping point altitude? + Vector3f stopping_point; + stopping_point.z = inertial_nav.get_altitude(); + wp_nav.get_wp_stopping_point_xy(stopping_point); - // initialise yaw - set_auto_yaw_mode(get_default_auto_yaw_mode(false)); + // no need to check return status because terrain data is not used + wp_nav.set_wp_destination(stopping_point, false); + + // initialise yaw + set_auto_yaw_mode(get_default_auto_yaw_mode(false)); } // initialise guided mode's velocity controller @@ -163,7 +173,32 @@ void Sub::guided_set_destination(const Vector3f& destination) guided_pos_control_start(); } - wp_nav.set_wp_destination(destination); + // no need to check return status because terrain data is not used + wp_nav.set_wp_destination(destination, false); + + // log target + Log_Write_GuidedTarget(guided_mode, destination, Vector3f()); +} + +// sets guided mode's target from a Location object +// returns false if destination could not be set (probably caused by missing terrain data) +bool Sub::guided_set_destination(const Location_Class& dest_loc) +{ + // ensure we are in position control mode + if (guided_mode != Guided_WP) { + guided_pos_control_start(); + } + + if (!wp_nav.set_wp_destination(dest_loc)) { + // failure to set destination can only be because of missing terrain data + Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); + // failure is propagated to GCS with NAK + return false; + } + + // log target + Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f()); + return true; } // guided_set_velocity - sets guided mode's target velocity @@ -270,7 +305,7 @@ void Sub::guided_takeoff_run() motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller - wp_nav.update_wpnav(); + failsafe_terrain_set_status(wp_nav.update_wpnav()); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); @@ -306,7 +341,7 @@ void Sub::guided_pos_control_run() motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller - wp_nav.update_wpnav(); + failsafe_terrain_set_status(wp_nav.update_wpnav()); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); diff --git a/ArduSub/control_land.cpp b/ArduSub/control_land.cpp index 825a928d01..59c03a9c73 100644 --- a/ArduSub/control_land.cpp +++ b/ArduSub/control_land.cpp @@ -221,8 +221,21 @@ float Sub::get_land_descent_speed() #else bool sonar_ok = false; #endif + + // get position controller's target altitude above terrain + Location_Class target_loc = pos_control.get_pos_target(); + int32_t target_alt_cm; + + // get altitude target above home by default + target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, target_alt_cm); + + // try to use terrain if enabled + if (terrain_use() && !target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_alt_cm)) { + Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); + } + // if we are above 10m and the sonar does not sense anything perform regular alt hold descent - if (pos_control.get_pos_target().z <= pv_alt_above_origin(LAND_START_ALT) && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) { + if (target_alt_cm <= LAND_START_ALT && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) { if (g.land_speed_high > 0) { // user has asked for a different landing speed than normal descent rate return -abs(g.land_speed_high); diff --git a/ArduSub/control_rtl.cpp b/ArduSub/control_rtl.cpp index dc0a1146eb..c94ef8dbcb 100644 --- a/ArduSub/control_rtl.cpp +++ b/ArduSub/control_rtl.cpp @@ -13,6 +13,7 @@ bool Sub::rtl_init(bool ignore_checks) { if (position_ok() || ignore_checks) { + rtl_build_path(true); rtl_climb_start(); return true; }else{ @@ -20,6 +21,18 @@ bool Sub::rtl_init(bool ignore_checks) } } +// re-start RTL with terrain following disabled +void Sub::rtl_restart_without_terrain() +{ + // log an error + Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_RESTARTED_RTL); + if (rtl_path.terrain_used) { + rtl_build_path(false); + rtl_climb_start(); + gcs_send_text(MAV_SEVERITY_CRITICAL,"Restarting RTL - Terrain data missing"); + } +} + // rtl_run - runs the return-to-launch controller // should be called at 100hz or more void Sub::rtl_run() @@ -79,7 +92,6 @@ void Sub::rtl_climb_start() { rtl_state = RTL_InitialClimb; rtl_state_complete = false; - rtl_alt = get_RTL_alt(); // initialise waypoint and spline controller wp_nav.wp_and_spline_init(); @@ -89,22 +101,13 @@ void Sub::rtl_climb_start() wp_nav.set_speed_xy(g.rtl_speed_cms); } - // get horizontal stopping point - Vector3f destination; - wp_nav.get_wp_stopping_point_xy(destination); - -#if AC_RALLY == ENABLED - // rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes - Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt); - rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home - rally_point.alt = MAX(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home - destination.z = pv_alt_above_origin(rally_point.alt); -#else - destination.z = pv_alt_above_origin(rtl_alt); -#endif - // set the destination - wp_nav.set_wp_destination(destination); + if (!wp_nav.set_wp_destination(rtl_path.climb_target)) { + // this should not happen because rtl_build_path will have checked terrain data was available + Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); + set_mode(LAND, MODE_REASON_TERRAIN_FAILSAFE); + return; + } wp_nav.set_fast_waypoint(true); // hold current yaw during initial climb @@ -117,19 +120,10 @@ void Sub::rtl_return_start() rtl_state = RTL_ReturnHome; rtl_state_complete = false; - // set target to above home/rally point -#if AC_RALLY == ENABLED - // rally_point will be the nearest rally point or home. uses absolute altitudes - Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt); - rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home - rally_point.alt = MAX(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home - Vector3f destination = pv_location_to_vector(rally_point); -#else - Vector3f destination = pv_location_to_vector(ahrs.get_home()); - destination.z = pv_alt_above_origin(rtl_alt); -#endif - - wp_nav.set_wp_destination(destination); + if (!wp_nav.set_wp_destination(rtl_path.return_target)) { + // failure must be caused by missing terrain data, restart RTL + rtl_restart_without_terrain(); + } // initialise yaw to point home (maybe) set_auto_yaw_mode(get_default_auto_yaw_mode(true)); @@ -164,7 +158,7 @@ void Sub::rtl_climb_return_run() motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller - wp_nav.update_wpnav(); + failsafe_terrain_set_status(wp_nav.update_wpnav()); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); @@ -182,7 +176,7 @@ void Sub::rtl_climb_return_run() rtl_state_complete = wp_nav.reached_wp_destination(); } -// rtl_return_start - initialise return to home +// rtl_loiterathome_start - initialise return to home void Sub::rtl_loiterathome_start() { rtl_state = RTL_LoiterAtHome; @@ -226,7 +220,7 @@ void Sub::rtl_loiterathome_run() motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller - wp_nav.update_wpnav(); + failsafe_terrain_set_status(wp_nav.update_wpnav()); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); @@ -321,14 +315,14 @@ void Sub::rtl_descent_run() wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call z-axis position controller - pos_control.set_alt_target_with_slew(pv_alt_above_origin(g.rtl_alt_final), G_Dt); + pos_control.set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt); pos_control.update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); // check if we've reached within 20cm of final altitude - rtl_state_complete = fabsf(pv_alt_above_origin(g.rtl_alt_final) - inertial_nav.get_altitude()) < 20.0f; + rtl_state_complete = fabsf(rtl_path.descent_target.alt - current_loc.alt) < 20.0f; } // rtl_loiterathome_start - initialise controllers to loiter over home @@ -423,21 +417,87 @@ void Sub::rtl_land_run() rtl_state_complete = ap.land_complete; } -// get_RTL_alt - return altitude which vehicle should return home at -// altitude is in cm above home -float Sub::get_RTL_alt() +void Sub::rtl_build_path(bool terrain_following_allowed) { + // origin point is our stopping point + Vector3f stopping_point; + pos_control.get_stopping_point_xy(stopping_point); + pos_control.get_stopping_point_z(stopping_point); + rtl_path.origin_point = Location_Class(stopping_point); + rtl_path.origin_point.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME); + + // set return target to nearest rally point or home position +#if AC_RALLY == ENABLED + rtl_path.return_target = rally.calc_best_rally_or_home_location(current_loc, ahrs.get_home().alt); +#else + rtl_path.return_target = ahrs.get_home(); +#endif + + // compute return altitude + rtl_compute_return_alt(rtl_path.origin_point, rtl_path.return_target, terrain_following_allowed); + + // climb target is above our origin point at the return altitude + rtl_path.climb_target = Location_Class(rtl_path.origin_point.lat, rtl_path.origin_point.lng, rtl_path.return_target.alt, rtl_path.return_target.get_alt_frame()); + + // descent target is below return target at rtl_alt_final + rtl_path.descent_target = Location_Class(rtl_path.return_target.lat, rtl_path.return_target.lng, g.rtl_alt_final, Location_Class::ALT_FRAME_ABOVE_HOME); + + // set land flag + rtl_path.land = g.rtl_alt_final <= 0; +} + +// return altitude in cm above home at which vehicle should return home +// rtl_origin_point is the stopping point of the vehicle when rtl is initiated +// rtl_return_target is the home or rally point that the vehicle is returning to. It's lat, lng and alt values must already have been filled in before this function is called +// rtl_return_target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set) +void Sub::rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target, bool terrain_following_allowed) +{ + float rtl_return_dist_cm = rtl_return_target.get_distance(rtl_origin_point) * 100.0f; + + // curr_alt is current altitude above home or above terrain depending upon use_terrain + int32_t curr_alt = current_loc.alt; + + // decide if we should use terrain altitudes + rtl_path.terrain_used = terrain_use() && terrain_following_allowed; + if (rtl_path.terrain_used) { + // attempt to retrieve terrain alt for current location, stopping point and origin + int32_t origin_terr_alt, return_target_terr_alt; + if (!rtl_origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, origin_terr_alt) || + !rtl_origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, return_target_terr_alt) || + !current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) { + rtl_path.terrain_used = false; + Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); + } + } + // maximum of current altitude + climb_min and rtl altitude - float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), g.rtl_altitude); - ret = MAX(ret, RTL_ALT_MIN); + float ret = MAX(curr_alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN)); + + // don't allow really shallow slopes + if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) { + ret = MAX(curr_alt, MIN(ret, MAX(rtl_return_dist_cm*g.rtl_cone_slope, curr_alt+RTL_ABS_MIN_CLIMB))); + } #if AC_FENCE == ENABLED // ensure not above fence altitude if alt fence is enabled + // Note: we are assuming the fence alt is the same frame as ret if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) { ret = MIN(ret, fence.get_safe_alt()*100.0f); } #endif - return ret; + // ensure we do not descend + ret = MAX(ret, curr_alt); + + // convert return-target to alt-above-home or alt-above-terrain + if (!rtl_path.terrain_used || !rtl_return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_TERRAIN)) { + if (!rtl_return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME)) { + // this should never happen but just in case + rtl_return_target.set_alt_cm(0, Location_Class::ALT_FRAME_ABOVE_HOME); + } + } + + // add ret to altitude + rtl_return_target.alt += ret; } diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 7138cd74f2..8ff18f1987 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -125,7 +125,8 @@ enum mode_reason_t { MODE_REASON_GPS_GLITCH, MODE_REASON_MISSION_END, MODE_REASON_THROTTLE_LAND_ESCAPE, - MODE_REASON_FENCE_BREACH + MODE_REASON_FENCE_BREACH, + MODE_REASON_TERRAIN_FAILSAFE }; // Tuning enumeration @@ -285,6 +286,7 @@ enum ThrowModeState { #define LOG_PARAMTUNE_MSG 0x1F #define LOG_HELI_MSG 0x20 #define LOG_PRECLAND_MSG 0x21 +#define LOG_GUIDEDTARGET_MSG 0x22 #define MASK_LOG_ATTITUDE_FAST (1<<0) #define MASK_LOG_ATTITUDE_MED (1<<1) @@ -391,6 +393,9 @@ enum ThrowModeState { #define ERROR_SUBSYSTEM_BARO 18 #define ERROR_SUBSYSTEM_CPU 19 #define ERROR_SUBSYSTEM_FAILSAFE_ADSB 20 +#define ERROR_SUBSYSTEM_TERRAIN 21 +#define ERROR_SUBSYSTEM_NAVIGATION 22 +#define ERROR_SUBSYSTEM_FAILSAFE_TERRAIN 23 // general error codes #define ERROR_CODE_ERROR_RESOLVED 0 #define ERROR_CODE_FAILED_TO_INITIALISE 1 @@ -409,6 +414,12 @@ enum ThrowModeState { #define ERROR_CODE_CRASH_CHECK_LOSS_OF_CONTROL 2 // subsystem specific error codes -- flip #define ERROR_CODE_FLIP_ABANDONED 2 +// subsystem specific error codes -- terrain +#define ERROR_CODE_MISSING_TERRAIN_DATA 2 +// subsystem specific error codes -- navigation +#define ERROR_CODE_FAILED_TO_SET_DESTINATION 2 +#define ERROR_CODE_RESTARTED_RTL 3 +#define ERROR_CODE_FAILED_CIRCLE_INIT 4 // parachute failed to deploy because of low altitude or landed #define ERROR_CODE_PARACHUTE_TOO_LOW 2 diff --git a/ArduSub/events.cpp b/ArduSub/events.cpp index ce97c8f799..318ddca5c1 100644 --- a/ArduSub/events.cpp +++ b/ArduSub/events.cpp @@ -123,6 +123,62 @@ void Sub::failsafe_gcs_off_event(void) Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED); } +// executes terrain failsafe if data is missing for longer than a few seconds +// missing_data should be set to true if the vehicle failed to navigate because of missing data, false if navigation is proceeding successfully +void Sub::failsafe_terrain_check() +{ + // trigger with 5 seconds of failures while in AUTO mode + bool valid_mode = (control_mode == AUTO || control_mode == GUIDED || control_mode == RTL); + bool timeout = (failsafe.terrain_last_failure_ms - failsafe.terrain_first_failure_ms) > FS_TERRAIN_TIMEOUT_MS; + bool trigger_event = valid_mode && timeout; + + // check for clearing of event + if (trigger_event != failsafe.terrain) { + if (trigger_event) { + failsafe_terrain_on_event(); + } else { + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_ERROR_RESOLVED); + failsafe.terrain = false; + } + } +} + +// set terrain data status (found or not found) +void Sub::failsafe_terrain_set_status(bool data_ok) +{ + uint32_t now = millis(); + + // record time of first and latest failures (i.e. duration of failures) + if (!data_ok) { + failsafe.terrain_last_failure_ms = now; + if (failsafe.terrain_first_failure_ms == 0) { + failsafe.terrain_first_failure_ms = now; + } + } else { + // failures cleared after 0.1 seconds of persistent successes + if (now - failsafe.terrain_last_failure_ms > 100) { + failsafe.terrain_last_failure_ms = 0; + failsafe.terrain_first_failure_ms = 0; + } + } +} + +// terrain failsafe action +void Sub::failsafe_terrain_on_event() +{ + failsafe.terrain = true; + gcs_send_text(MAV_SEVERITY_CRITICAL,"Failsafe: Terrain data missing"); + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_FAILSAFE_OCCURRED); + + if (should_disarm_on_failsafe()) { + init_disarm_motors(); + } else if (control_mode == RTL) { + rtl_restart_without_terrain(); + } else { + set_mode_RTL_or_land_with_pause(MODE_REASON_TERRAIN_FAILSAFE); + } +} + // set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts // this is always called from a failsafe so we trigger notification to pilot void Sub::set_mode_RTL_or_land_with_pause(mode_reason_t reason) diff --git a/ArduSub/inertia.cpp b/ArduSub/inertia.cpp index d0f8407aea..5853ff0f33 100644 --- a/ArduSub/inertia.cpp +++ b/ArduSub/inertia.cpp @@ -7,11 +7,11 @@ void Sub::read_inertia() { // inertial altitude estimates inertial_nav.update(G_Dt); -} -// read_inertial_altitude - pull altitude and climb rate from inertial nav library -void Sub::read_inertial_altitude() -{ + // pull position from interial nav library + current_loc.lng = inertial_nav.get_longitude(); + current_loc.lat = inertial_nav.get_latitude(); + // exit immediately if we do not have an altitude estimate if (!inertial_nav.get_filter_status().flags.vert_pos) { return; diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 4659cf5b26..4dbb0d382f 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -234,8 +234,8 @@ void Sub::init_disarm_motors() gcs_send_text(MAV_SEVERITY_INFO, "Disarming motors"); #endif - // save compass offsets learned by the EKF - if (ahrs.use_compass()) { + // save compass offsets learned by the EKF if enabled + if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) { for(uint8_t i=0; i