From 366051c39ff184ce0fbf1f5bf484ffd09b6c8d56 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 15 Mar 2019 12:44:27 +1100 Subject: [PATCH] Copter: use enum class for AltFrame enumeration --- ArduCopter/GCS_Mavlink.cpp | 2 +- ArduCopter/inertia.cpp | 8 ++++---- ArduCopter/mode.cpp | 2 +- ArduCopter/mode_auto.cpp | 16 ++++++++-------- ArduCopter/mode_guided.cpp | 2 +- ArduCopter/mode_rtl.cpp | 20 ++++++++++---------- ArduCopter/precision_landing.cpp | 2 +- 7 files changed, 26 insertions(+), 26 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index d5ce0abd19..06a1dc6276 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1082,7 +1082,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) if (!check_latlng(packet.lat_int, packet.lon_int)) { break; } - Location::ALT_FRAME frame; + Location::AltFrame frame; if (!mavlink_coordinate_frame_to_location_alt_frame(packet.coordinate_frame, frame)) { // unknown coordinate frame break; diff --git a/ArduCopter/inertia.cpp b/ArduCopter/inertia.cpp index 9e69a7dd64..002f935871 100644 --- a/ArduCopter/inertia.cpp +++ b/ArduCopter/inertia.cpp @@ -15,15 +15,15 @@ void Copter::read_inertia() return; } - Location::ALT_FRAME frame; + Location::AltFrame frame; if (ahrs.home_is_set()) { - frame = Location::ALT_FRAME_ABOVE_HOME; + frame = Location::AltFrame::ABOVE_HOME; } else { // without home use alt above the EKF origin - frame = Location::ALT_FRAME_ABOVE_ORIGIN; + frame = Location::AltFrame::ABOVE_ORIGIN; } current_loc.set_alt_cm(inertial_nav.get_altitude(), frame); - current_loc.change_alt_frame(Location::ALT_FRAME_ABOVE_HOME); + current_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME); // set flags and get velocity climb_rate = inertial_nav.get_velocity_z(); diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 48590837a9..57e6af0aad 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -408,7 +408,7 @@ int32_t Copter::Mode::get_alt_above_ground_cm(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::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) { + if (!navigating || !copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground)) { alt_above_ground = copter.current_loc.alt; } } diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index dd59d31a71..4093868009 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -149,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::ALT_FRAME_ABOVE_HOME, alt_target)) { + if (!dest.get_alt_cm(Location::AltFrame::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 @@ -158,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::ALT_FRAME_ABOVE_HOME); + dest.set_alt_cm(copter.current_loc.alt, Location::AltFrame::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::ALT_FRAME_ABOVE_HOME); + dest.set_alt_cm(100, Location::AltFrame::ABOVE_HOME); } // set waypoint controller target @@ -1052,14 +1052,14 @@ Location Copter::ModeAuto::terrain_adjusted_location(const AP_Mission::Mission_C // 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::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) && - target_loc.get_alt_cm(Location::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) { + if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, curr_terr_alt_cm) && + target_loc.get_alt_cm(Location::AltFrame::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::ALT_FRAME_ABOVE_TERRAIN); + target_loc.set_alt_cm(curr_terr_alt_cm, Location::AltFrame::ABOVE_TERRAIN); } else { // set target altitude to current altitude above home - target_loc.set_alt_cm(copter.current_loc.alt, Location::ALT_FRAME_ABOVE_HOME); + target_loc.set_alt_cm(copter.current_loc.alt, Location::AltFrame::ABOVE_HOME); } return target_loc; } @@ -1241,7 +1241,7 @@ void Copter::ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) target_loc.lng = copter.current_loc.lng; } - if (!target_loc.get_alt_cm(Location::ALT_FRAME_ABOVE_HOME, loiter_to_alt.alt)) { + if (!target_loc.get_alt_cm(Location::AltFrame::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"); diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index c51f9a243a..b744d9122a 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -53,7 +53,7 @@ bool Copter::ModeGuided::do_user_takeoff_start(float final_alt_above_home) // initialise wpnav destination Location target_loc = copter.current_loc; - target_loc.set_alt_cm(final_alt_above_home, Location::ALT_FRAME_ABOVE_HOME); + target_loc.set_alt_cm(final_alt_above_home, Location::AltFrame::ABOVE_HOME); if (!wp_nav->set_wp_destination(target_loc)) { // failure to set destination can only be because of missing terrain data diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index a528b3345c..4b0fcfc244 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -393,7 +393,7 @@ void Copter::ModeRTL::build_path(bool terrain_following_allowed) pos_control->get_stopping_point_xy(stopping_point); pos_control->get_stopping_point_z(stopping_point); rtl_path.origin_point = Location(stopping_point); - rtl_path.origin_point.change_alt_frame(Location::ALT_FRAME_ABOVE_HOME); + rtl_path.origin_point.change_alt_frame(Location::AltFrame::ABOVE_HOME); // compute return target compute_return_target(terrain_following_allowed); @@ -402,7 +402,7 @@ void Copter::ModeRTL::build_path(bool terrain_following_allowed) 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(rtl_path.return_target.lat, rtl_path.return_target.lng, g.rtl_alt_final, Location::ALT_FRAME_ABOVE_HOME); + rtl_path.descent_target = Location(rtl_path.return_target.lat, rtl_path.return_target.lng, g.rtl_alt_final, Location::AltFrame::ABOVE_HOME); // set land flag rtl_path.land = g.rtl_alt_final <= 0; @@ -428,19 +428,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::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)) { + if (!rtl_path.origin_point.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, origin_terr_alt) || + !rtl_path.return_target.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, return_target_terr_alt) || + !copter.current_loc.get_alt_cm(Location::AltFrame::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::ALT_FRAME_ABOVE_TERRAIN)) { - if (!rtl_path.return_target.change_alt_frame(Location::ALT_FRAME_ABOVE_HOME)) { + if (!rtl_path.terrain_used || !rtl_path.return_target.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN)) { + if (!rtl_path.return_target.change_alt_frame(Location::AltFrame::ABOVE_HOME)) { // this should never happen but just in case - rtl_path.return_target.set_alt_cm(0, Location::ALT_FRAME_ABOVE_HOME); + rtl_path.return_target.set_alt_cm(0, Location::AltFrame::ABOVE_HOME); } rtl_path.terrain_used = false; } @@ -462,7 +462,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::ALT_FRAME_ABOVE_TERRAIN : Location::ALT_FRAME_ABOVE_HOME); + rtl_path.return_target.set_alt_cm(target_alt, rtl_path.terrain_used ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_HOME); #if AC_FENCE == ENABLED // ensure not above fence altitude if alt fence is enabled @@ -472,7 +472,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::ALT_FRAME_ABOVE_HOME, target_alt)) { + if (rtl_path.return_target.get_alt_cm(Location::AltFrame::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 d507be7206..606a97386e 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::ALT_FRAME_ABOVE_TERRAIN, height_above_ground_cm)) { + if (!current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, height_above_ground_cm)) { height_above_ground_cm = current_loc.alt; } }