diff --git a/ArduCopter/AP_Rally.cpp b/ArduCopter/AP_Rally.cpp index 22e0d59333..574a17d9f7 100644 --- a/ArduCopter/AP_Rally.cpp +++ b/ArduCopter/AP_Rally.cpp @@ -22,8 +22,7 @@ bool AP_Rally_Copter::is_valid(const Location &rally_point) const { #if AC_FENCE == ENABLED - Location_Class rally_loc(rally_point); - if (!copter.fence.check_destination_within_fence(rally_loc)) { + if (!copter.fence.check_destination_within_fence(rally_point)) { return false; } #endif diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index bd3760bd95..7f63d1c741 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -440,7 +440,7 @@ private: // 3D Location vectors // Current location of the vehicle (altitude is relative to home) - Location_Class current_loc; + Location current_loc; // IMU variables // Integration time (in seconds) for the gyros (DCM algorithm) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 0fc8154a57..b337d54f19 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -96,7 +96,7 @@ MAV_STATE GCS_MAVLINK_Copter::system_status() const void GCS_MAVLINK_Copter::send_position_target_global_int() { - Location_Class target; + Location target; if (!copter.flightmode->get_wp(target)) { return; } diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index 1463bbefdd..980c1c3e52 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -192,8 +192,6 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw if (copter.mode_auto.mission.num_commands() == 0) { // set our location ID to 16, MAV_CMD_NAV_WAYPOINT cmd.id = MAV_CMD_NAV_TAKEOFF; - cmd.p1 = 0; - memset(&cmd.content.location, 0, sizeof(cmd.content.location)); cmd.content.location.alt = MAX(copter.current_loc.alt,100); // use the current altitude for the target alt for takeoff. diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 4d042c293d..2213462797 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -400,7 +400,7 @@ int32_t Copter::Mode::get_alt_above_ground(void) alt_above_ground = copter.rangefinder_state.alt_cm_filt.get(); } else { bool navigating = pos_control->is_active_xy(); - if (!navigating || !copter.current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) { + if (!navigating || !copter.current_loc.get_alt_cm(Location::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) { alt_above_ground = copter.current_loc.alt; } } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 006e80b7ec..40ce4c23aa 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -102,7 +102,7 @@ protected: virtual uint32_t wp_distance() const { return 0; } virtual int32_t wp_bearing() const { return 0; } virtual float crosstrack_error() const { return 0.0f;} - virtual bool get_wp(Location_Class &loc) { return false; }; + virtual bool get_wp(Location &loc) { return false; }; virtual bool in_guided_mode() const { return false; } // pilot input processing @@ -290,13 +290,13 @@ public: void rtl_start(); void takeoff_start(const Location& dest_loc); void wp_start(const Vector3f& destination); - void wp_start(const Location_Class& dest_loc); + void wp_start(const Location& dest_loc); void land_start(); void land_start(const Vector3f& destination); - void circle_movetoedge_start(const Location_Class &circle_center, float radius_m); + void circle_movetoedge_start(const Location &circle_center, float radius_m); void circle_start(); void spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_spline_destination); - void 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 spline_start(const Location& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location& next_destination); void nav_guided_start(); bool landing_gear_should_be_deployed() const override; @@ -323,7 +323,7 @@ protected: uint32_t wp_distance() const override; int32_t wp_bearing() const override; float crosstrack_error() const override { return wp_nav->crosstrack_error();} - bool get_wp(Location_Class &loc) override; + bool get_wp(Location &loc) override; void run_autopilot() override; private: @@ -342,7 +342,7 @@ private: void loiter_run(); void loiter_to_alt_run(); - Location_Class loc_from_cmd(const AP_Mission::Mission_Command& cmd) const; + Location loc_from_cmd(const AP_Mission::Mission_Command& cmd) const; void payload_place_start(const Vector3f& destination); void payload_place_run(); @@ -353,7 +353,7 @@ private: AutoMode _mode = Auto_TakeOff; // controls which auto controller is run - Location_Class terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const; + Location terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const; void do_takeoff(const AP_Mission::Mission_Command& cmd); void do_nav_wp(const AP_Mission::Mission_Command& cmd); @@ -402,7 +402,7 @@ private: #endif bool verify_nav_delay(const AP_Mission::Mission_Command& cmd); - 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 auto_spline_start(const Location& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location& next_destination); // Loiter control uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds) @@ -698,8 +698,8 @@ public: void set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads); bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); - bool set_destination(const Location_Class& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); - bool get_wp(Location_Class &loc) override; + bool set_destination(const Location& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); + bool get_wp(Location &loc) override; void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true); bool set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); @@ -927,10 +927,10 @@ private: 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; + Location origin_point; + Location climb_target; + Location return_target; + Location descent_target; bool land; bool terrain_used; } rtl_path; @@ -1142,7 +1142,7 @@ protected: const char *name4() const override { return "FOLL"; } uint32_t wp_distance() const override; int32_t wp_bearing() const override; - bool get_wp(Location_Class &loc) override; + bool get_wp(Location &loc) override; uint32_t last_log_ms; // system time of last time desired velocity was logging }; diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index c45b25d6e1..4c64ec1ee3 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -141,8 +141,7 @@ void Copter::ModeAuto::takeoff_start(const Location& dest_loc) { _mode = Auto_TakeOff; - // convert location to class - Location_Class dest(dest_loc); + Location dest(dest_loc); // set horizontal target dest.lat = copter.current_loc.lat; @@ -150,7 +149,7 @@ void Copter::ModeAuto::takeoff_start(const Location& dest_loc) // get altitude target int32_t alt_target; - if (!dest.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target)) { + if (!dest.get_alt_cm(Location::ALT_FRAME_ABOVE_HOME, alt_target)) { // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); // fall back to altitude above current altitude @@ -159,11 +158,11 @@ void Copter::ModeAuto::takeoff_start(const Location& dest_loc) // sanity check target if (alt_target < copter.current_loc.alt) { - dest.set_alt_cm(copter.current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME); + dest.set_alt_cm(copter.current_loc.alt, Location::ALT_FRAME_ABOVE_HOME); } // Note: if taking off from below home this could cause a climb to an unexpectedly high altitude if (alt_target < 100) { - dest.set_alt_cm(100, Location_Class::ALT_FRAME_ABOVE_HOME); + dest.set_alt_cm(100, Location::ALT_FRAME_ABOVE_HOME); } // set waypoint controller target @@ -199,7 +198,7 @@ void Copter::ModeAuto::wp_start(const Vector3f& destination) } // auto_wp_start - initialises waypoint controller to implement flying to a particular destination -void Copter::ModeAuto::wp_start(const Location_Class& dest_loc) +void Copter::ModeAuto::wp_start(const Location& dest_loc) { _mode = Auto_WP; @@ -248,7 +247,7 @@ void Copter::ModeAuto::land_start(const Vector3f& destination) // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location // we assume the caller has performed all required GPS_ok checks -void Copter::ModeAuto::circle_movetoedge_start(const Location_Class &circle_center, float radius_m) +void Copter::ModeAuto::circle_movetoedge_start(const Location &circle_center, float radius_m) { // convert location to vector from ekf origin Vector3f circle_center_neu; @@ -274,8 +273,8 @@ void Copter::ModeAuto::circle_movetoedge_start(const Location_Class &circle_cent // set the state to move to the edge of the circle _mode = Auto_CircleMoveToEdge; - // convert circle_edge_neu to Location_Class - Location_Class circle_edge(circle_edge_neu); + // convert circle_edge_neu to Location + Location circle_edge(circle_edge_neu); // convert altitude to same as command circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame()); @@ -316,9 +315,9 @@ void Copter::ModeAuto::circle_start() // auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller // seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE. If Straight or Spline the next_destination should be provided -void Copter::ModeAuto::spline_start(const Location_Class& destination, bool stopped_at_start, +void Copter::ModeAuto::spline_start(const Location& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, - const Location_Class& next_destination) + const Location& next_destination) { _mode = Auto_Spline; @@ -544,7 +543,7 @@ bool Copter::ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd) case MAV_CMD_NAV_WAYPOINT: { // set wp_nav's destination - Location_Class dest(cmd.content.location); + Location dest(cmd.content.location); return copter.mode_guided.set_destination(dest); } @@ -570,7 +569,7 @@ int32_t Copter::ModeAuto::wp_bearing() const return wp_nav->get_wp_bearing_to_destination(); } -bool Copter::ModeAuto::get_wp(Location_Class& destination) +bool Copter::ModeAuto::get_wp(Location& destination) { switch (_mode) { case Auto_NavGuided: @@ -1035,21 +1034,21 @@ void Copter::ModeAuto::payload_place_run_descend() // terrain_adjusted_location: returns a Location with lat/lon from cmd // and altitude from our current altitude adjusted for location -Location_Class Copter::ModeAuto::terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const +Location Copter::ModeAuto::terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const { // convert to location class - Location_Class target_loc(cmd.content.location); + Location target_loc(cmd.content.location); // decide if we will use terrain following int32_t curr_terr_alt_cm, target_terr_alt_cm; - if (copter.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)) { + if (copter.current_loc.get_alt_cm(Location::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) && + target_loc.get_alt_cm(Location::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); + target_loc.set_alt_cm(curr_terr_alt_cm, Location::ALT_FRAME_ABOVE_TERRAIN); } else { // set target altitude to current altitude above home - target_loc.set_alt_cm(copter.current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME); + target_loc.set_alt_cm(copter.current_loc.alt, Location::ALT_FRAME_ABOVE_HOME); } return target_loc; } @@ -1065,9 +1064,9 @@ void Copter::ModeAuto::do_takeoff(const AP_Mission::Mission_Command& cmd) takeoff_start(cmd.content.location); } -Location_Class Copter::ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd) const +Location Copter::ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd) const { - Location_Class ret(cmd.content.location); + Location ret(cmd.content.location); // use current lat, lon if zero if (ret.lat == 0 && ret.lng == 0) { @@ -1092,7 +1091,7 @@ Location_Class Copter::ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& // do_nav_wp - initiate move to next waypoint void Copter::ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) { - Location_Class target_loc = loc_from_cmd(cmd); + Location target_loc = loc_from_cmd(cmd); // this will be used to remember the time in millis after we reach or pass the WP. loiter_time = 0; @@ -1119,7 +1118,7 @@ void Copter::ModeAuto::do_land(const AP_Mission::Mission_Command& cmd) // set state to fly to location land_state = LandStateType_FlyToLocation; - Location_Class target_loc = terrain_adjusted_location(cmd); + const Location target_loc = terrain_adjusted_location(cmd); wp_start(target_loc); } else { @@ -1136,14 +1135,14 @@ void Copter::ModeAuto::do_land(const AP_Mission::Mission_Command& cmd) void Copter::ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) { // convert back to location - Location_Class target_loc(cmd.content.location); + Location target_loc(cmd.content.location); // use current location if not provided if (target_loc.lat == 0 && target_loc.lng == 0) { // To-Do: make this simpler Vector3f temp_pos; copter.wp_nav->get_wp_stopping_point_xy(temp_pos); - Location_Class temp_loc(temp_pos); + const Location temp_loc(temp_pos); target_loc.lat = temp_loc.lat; target_loc.lng = temp_loc.lng; } @@ -1169,7 +1168,7 @@ void Copter::ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cm // do_circle - initiate moving in a circle void Copter::ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd) { - Location_Class circle_center = loc_from_cmd(cmd); + const Location circle_center = loc_from_cmd(cmd); // calculate radius uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1 @@ -1200,14 +1199,13 @@ void Copter::ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) // if we aren't navigating to a location then we have to adjust // altitude for current location - Location_Class target_loc(cmd.content.location); - const Location_Class ¤t_loc = copter.current_loc; + Location target_loc(cmd.content.location); if (target_loc.lat == 0 && target_loc.lng == 0) { - target_loc.lat = current_loc.lat; - target_loc.lng = current_loc.lng; + target_loc.lat = copter.current_loc.lat; + target_loc.lng = copter.current_loc.lng; } - if (!target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, loiter_to_alt.alt)) { + if (!target_loc.get_alt_cm(Location::ALT_FRAME_ABOVE_HOME, loiter_to_alt.alt)) { loiter_to_alt.reached_destination_xy = true; loiter_to_alt.reached_alt = true; gcs().send_text(MAV_SEVERITY_INFO, "bad do_loiter_to_alt"); @@ -1230,7 +1228,7 @@ void Copter::ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) // do_spline_wp - initiate move to next waypoint void Copter::ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd) { - Location_Class target_loc = loc_from_cmd(cmd); + const Location target_loc = loc_from_cmd(cmd); // this will be used to remember the time in millis after we reach or pass the WP. loiter_time = 0; @@ -1254,7 +1252,7 @@ void Copter::ModeAuto::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; + Location 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 @@ -1451,7 +1449,7 @@ void Copter::ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd) // set state to fly to location nav_payload_place.state = PayloadPlaceStateType_FlyToLocation; - Location_Class target_loc = terrain_adjusted_location(cmd); + const Location target_loc = terrain_adjusted_location(cmd); wp_start(target_loc); } else { @@ -1654,7 +1652,7 @@ bool Copter::ModeAuto::verify_payload_place() } FALLTHROUGH; case PayloadPlaceStateType_Ascending_Start: { - Location_Class target_loc = inertial_nav.get_position(); + Location target_loc = inertial_nav.get_position(); target_loc.alt = nav_payload_place.descend_start_altitude; wp_start(target_loc); nav_payload_place.state = PayloadPlaceStateType_Ascending; diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index 2fc4d057d1..ed37061e85 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -163,7 +163,7 @@ int32_t Copter::ModeFollow::wp_bearing() const /* get target position for mavlink reporting */ -bool Copter::ModeFollow::get_wp(Location_Class &loc) +bool Copter::ModeFollow::get_wp(Location &loc) { float dist = g2.follow.get_distance_to_target(); float bearing = g2.follow.get_bearing_to_target(); diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 96ae5df141..90286ceaca 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -54,8 +54,8 @@ bool Copter::ModeGuided::do_user_takeoff_start(float final_alt_above_home) guided_mode = Guided_TakeOff; // initialise wpnav destination - Location_Class target_loc = copter.current_loc; - target_loc.set_alt_cm(final_alt_above_home, Location_Class::ALT_FRAME_ABOVE_HOME); + Location target_loc = copter.current_loc; + target_loc.set_alt_cm(final_alt_above_home, Location::ALT_FRAME_ABOVE_HOME); if (!wp_nav->set_wp_destination(target_loc)) { // failure to set destination can only be because of missing terrain data @@ -182,7 +182,7 @@ bool Copter::ModeGuided::set_destination(const Vector3f& destination, bool use_y #if AC_FENCE == ENABLED // reject destination if outside the fence - Location_Class dest_loc(destination); + const Location dest_loc(destination); if (!copter.fence.check_destination_within_fence(dest_loc)) { copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK @@ -201,7 +201,7 @@ bool Copter::ModeGuided::set_destination(const Vector3f& destination, bool use_y return true; } -bool Copter::ModeGuided::get_wp(Location_Class& destination) +bool Copter::ModeGuided::get_wp(Location& destination) { if (guided_mode != Guided_WP) { return false; @@ -212,7 +212,7 @@ bool Copter::ModeGuided::get_wp(Location_Class& destination) // sets guided mode's target from a Location object // returns false if destination could not be set (probably caused by missing terrain data) // or if the fence is enabled and guided waypoint is outside the fence -bool Copter::ModeGuided::set_destination(const Location_Class& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +bool Copter::ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) { // ensure we are in position control mode if (guided_mode != Guided_WP) { @@ -275,7 +275,7 @@ bool Copter::ModeGuided::set_destination_posvel(const Vector3f& destination, con #if AC_FENCE == ENABLED // reject destination if outside the fence - Location_Class dest_loc(destination); + const Location dest_loc(destination); if (!copter.fence.check_destination_within_fence(dest_loc)) { copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 53439ed825..c1da00deeb 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -389,17 +389,17 @@ void Copter::ModeRTL::build_path(bool terrain_following_allowed) 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); + rtl_path.origin_point = Location(stopping_point); + rtl_path.origin_point.change_alt_frame(Location::ALT_FRAME_ABOVE_HOME); // compute return target compute_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()); + rtl_path.climb_target = Location(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); + rtl_path.descent_target = Location(rtl_path.return_target.lat, rtl_path.return_target.lng, g.rtl_alt_final, Location::ALT_FRAME_ABOVE_HOME); // set land flag rtl_path.land = g.rtl_alt_final <= 0; @@ -425,19 +425,19 @@ void Copter::ModeRTL::compute_return_target(bool 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_path.origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, origin_terr_alt) || - !rtl_path.return_target.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, return_target_terr_alt) || - !copter.current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) { + if (!rtl_path.origin_point.get_alt_cm(Location::ALT_FRAME_ABOVE_TERRAIN, origin_terr_alt) || + !rtl_path.return_target.get_alt_cm(Location::ALT_FRAME_ABOVE_TERRAIN, return_target_terr_alt) || + !copter.current_loc.get_alt_cm(Location::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) { rtl_path.terrain_used = false; copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); } } // convert return-target alt (which is an absolute alt) to alt-above-home or alt-above-terrain - if (!rtl_path.terrain_used || !rtl_path.return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_TERRAIN)) { - if (!rtl_path.return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME)) { + if (!rtl_path.terrain_used || !rtl_path.return_target.change_alt_frame(Location::ALT_FRAME_ABOVE_TERRAIN)) { + if (!rtl_path.return_target.change_alt_frame(Location::ALT_FRAME_ABOVE_HOME)) { // this should never happen but just in case - rtl_path.return_target.set_alt_cm(0, Location_Class::ALT_FRAME_ABOVE_HOME); + rtl_path.return_target.set_alt_cm(0, Location::ALT_FRAME_ABOVE_HOME); } rtl_path.terrain_used = false; } @@ -459,7 +459,7 @@ void Copter::ModeRTL::compute_return_target(bool terrain_following_allowed) } // set returned target alt to new target_alt - rtl_path.return_target.set_alt_cm(target_alt, rtl_path.terrain_used ? Location_Class::ALT_FRAME_ABOVE_TERRAIN : Location_Class::ALT_FRAME_ABOVE_HOME); + rtl_path.return_target.set_alt_cm(target_alt, rtl_path.terrain_used ? Location::ALT_FRAME_ABOVE_TERRAIN : Location::ALT_FRAME_ABOVE_HOME); #if AC_FENCE == ENABLED // ensure not above fence altitude if alt fence is enabled @@ -469,7 +469,7 @@ void Copter::ModeRTL::compute_return_target(bool terrain_following_allowed) // to apply the fence alt limit independently on the origin_point and return_target if ((copter.fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) { // get return target as alt-above-home so it can be compared to fence's alt - if (rtl_path.return_target.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, target_alt)) { + if (rtl_path.return_target.get_alt_cm(Location::ALT_FRAME_ABOVE_HOME, target_alt)) { float fence_alt = copter.fence.get_safe_alt_max()*100.0f; if (target_alt > fence_alt) { // reduce target alt to the fence alt diff --git a/ArduCopter/precision_landing.cpp b/ArduCopter/precision_landing.cpp index 89804b3f58..d507be7206 100644 --- a/ArduCopter/precision_landing.cpp +++ b/ArduCopter/precision_landing.cpp @@ -19,7 +19,7 @@ void Copter::update_precland() if (rangefinder_alt_ok()) { height_above_ground_cm = rangefinder_state.alt_cm; } else if (terrain_use()) { - if (!current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, height_above_ground_cm)) { + if (!current_loc.get_alt_cm(Location::ALT_FRAME_ABOVE_TERRAIN, height_above_ground_cm)) { height_above_ground_cm = current_loc.alt; } } diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index bcf25fd513..66b32acdd3 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -161,7 +161,7 @@ void Copter::init_ardupilot() // init Location class #if AP_TERRAIN_AVAILABLE && AC_TERRAIN - Location_Class::set_terrain(&terrain); + Location::set_terrain(&terrain); wp_nav->set_terrain(&terrain); #endif