aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: adjust for Location_Class and Location unification

Browse Source
This commit is contained in:
Peter Barker 2019-01-02 13:54:31 +11:00 committed by Peter Barker
parent 4e94d5c0ce
commit 845f015648
12 changed files with 73 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/AP_Rally.cpp
View File

@ -22,8 +22,7 @@
bool AP_Rally_Copter::is_valid(const Location &rally_point) const bool AP_Rally_Copter::is_valid(const Location &rally_point) const
{ {
#if AC_FENCE == ENABLED #if AC_FENCE == ENABLED
Location_Class rally_loc(rally_point); if (!copter.fence.check_destination_within_fence(rally_point)) {
if (!copter.fence.check_destination_within_fence(rally_loc)) {
return false; return false;
} }
#endif #endif

2
ArduCopter/Copter.h
View File

@ -440,7 +440,7 @@ private:
// 3D Location vectors // 3D Location vectors
// Current location of the vehicle (altitude is relative to home) // Current location of the vehicle (altitude is relative to home)
Location_Class current_loc; Location current_loc;
// IMU variables // IMU variables
// Integration time (in seconds) for the gyros (DCM algorithm) // Integration time (in seconds) for the gyros (DCM algorithm)

2
ArduCopter/GCS_Mavlink.cpp
View File

@ -96,7 +96,7 @@ MAV_STATE GCS_MAVLINK_Copter::system_status() const
void GCS_MAVLINK_Copter::send_position_target_global_int() void GCS_MAVLINK_Copter::send_position_target_global_int()
{ {
Location_Class target; Location target;
if (!copter.flightmode->get_wp(target)) { if (!copter.flightmode->get_wp(target)) {
return; return;
} }

2
ArduCopter/RC_Channel.cpp
View File

@ -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) { if (copter.mode_auto.mission.num_commands() == 0) {
// set our location ID to 16, MAV_CMD_NAV_WAYPOINT // set our location ID to 16, MAV_CMD_NAV_WAYPOINT
cmd.id = MAV_CMD_NAV_TAKEOFF; 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); cmd.content.location.alt = MAX(copter.current_loc.alt,100);
// use the current altitude for the target alt for takeoff. // use the current altitude for the target alt for takeoff.

2
ArduCopter/mode.cpp
View File

@ -400,7 +400,7 @@ int32_t Copter::Mode::get_alt_above_ground(void)
alt_above_ground = copter.rangefinder_state.alt_cm_filt.get(); alt_above_ground = copter.rangefinder_state.alt_cm_filt.get();
} else { } else {
bool navigating = pos_control->is_active_xy(); 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; alt_above_ground = copter.current_loc.alt;
} }
} }

30
ArduCopter/mode.h
View File

@ -102,7 +102,7 @@ protected:
virtual uint32_t wp_distance() const { return 0; } virtual uint32_t wp_distance() const { return 0; }
virtual int32_t wp_bearing() const { return 0; } virtual int32_t wp_bearing() const { return 0; }
virtual float crosstrack_error() const { return 0.0f;} 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; } virtual bool in_guided_mode() const { return false; }
// pilot input processing // pilot input processing
@ -290,13 +290,13 @@ public:
void rtl_start(); void rtl_start();
void takeoff_start(const Location& dest_loc); void takeoff_start(const Location& dest_loc);
void wp_start(const Vector3f& destination); 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();
void land_start(const Vector3f& destination); 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 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 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(); void nav_guided_start();
bool landing_gear_should_be_deployed() const override; bool landing_gear_should_be_deployed() const override;
@ -323,7 +323,7 @@ protected:
uint32_t wp_distance() const override; uint32_t wp_distance() const override;
int32_t wp_bearing() const override; int32_t wp_bearing() const override;
float crosstrack_error() const override { return wp_nav->crosstrack_error();} 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; void run_autopilot() override;
private: private:
@ -342,7 +342,7 @@ private:
void loiter_run(); void loiter_run();
void loiter_to_alt_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_start(const Vector3f& destination);
void payload_place_run(); void payload_place_run();
@ -353,7 +353,7 @@ private:
AutoMode _mode = Auto_TakeOff; // controls which auto controller is run 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_takeoff(const AP_Mission::Mission_Command& cmd);
void do_nav_wp(const AP_Mission::Mission_Command& cmd); void do_nav_wp(const AP_Mission::Mission_Command& cmd);
@ -402,7 +402,7 @@ private:
#endif #endif
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd); 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 // Loiter control
uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds) 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); 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 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 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_Class &loc) override; 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); 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); 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 { struct {
// NEU w/ Z element alt-above-ekf-origin unless use_terrain is true in which case Z element is alt-above-terrain // 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 origin_point;
Location_Class climb_target; Location climb_target;
Location_Class return_target; Location return_target;
Location_Class descent_target; Location descent_target;
bool land; bool land;
bool terrain_used; bool terrain_used;
} rtl_path; } rtl_path;
@ -1142,7 +1142,7 @@ protected:
const char *name4() const override { return "FOLL"; } const char *name4() const override { return "FOLL"; }
uint32_t wp_distance() const override; uint32_t wp_distance() const override;
int32_t wp_bearing() 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 uint32_t last_log_ms; // system time of last time desired velocity was logging
}; };

68
ArduCopter/mode_auto.cpp
View File

@ -141,8 +141,7 @@ void Copter::ModeAuto::takeoff_start(const Location& dest_loc)
{ {
_mode = Auto_TakeOff; _mode = Auto_TakeOff;
// convert location to class Location dest(dest_loc);
Location_Class dest(dest_loc);
// set horizontal target // set horizontal target
dest.lat = copter.current_loc.lat; dest.lat = copter.current_loc.lat;
@ -150,7 +149,7 @@ void Copter::ModeAuto::takeoff_start(const Location& dest_loc)
// get altitude target // get altitude target
int32_t alt_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 // 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); copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
// fall back to altitude above current altitude // fall back to altitude above current altitude
@ -159,11 +158,11 @@ void Copter::ModeAuto::takeoff_start(const Location& dest_loc)
// sanity check target // sanity check target
if (alt_target < copter.current_loc.alt) { 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 // Note: if taking off from below home this could cause a climb to an unexpectedly high altitude
if (alt_target < 100) { 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 // 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 // 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; _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 // 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 // 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 // convert location to vector from ekf origin
Vector3f circle_center_neu; 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 // set the state to move to the edge of the circle
_mode = Auto_CircleMoveToEdge; _mode = Auto_CircleMoveToEdge;
// convert circle_edge_neu to Location_Class // convert circle_edge_neu to Location
Location_Class circle_edge(circle_edge_neu); Location circle_edge(circle_edge_neu);
// convert altitude to same as command // convert altitude to same as command
circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame()); 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 // 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 // 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, AC_WPNav::spline_segment_end_type seg_end_type,
const Location_Class& next_destination) const Location& next_destination)
{ {
_mode = Auto_Spline; _mode = Auto_Spline;
@ -544,7 +543,7 @@ bool Copter::ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_NAV_WAYPOINT: case MAV_CMD_NAV_WAYPOINT:
{ {
// set wp_nav's destination // set wp_nav's destination
Location_Class dest(cmd.content.location); Location dest(cmd.content.location);
return copter.mode_guided.set_destination(dest); 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(); 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) { switch (_mode) {
case Auto_NavGuided: 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 // terrain_adjusted_location: returns a Location with lat/lon from cmd
// and altitude from our current altitude adjusted for location // 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 // convert to location class
Location_Class target_loc(cmd.content.location); Location target_loc(cmd.content.location);
// decide if we will use terrain following // decide if we will use terrain following
int32_t curr_terr_alt_cm, target_terr_alt_cm; 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) && if (copter.current_loc.get_alt_cm(Location::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) &&
target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_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); curr_terr_alt_cm = MAX(curr_terr_alt_cm,200);
// if using terrain, set target altitude to current altitude above terrain // 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 { } else {
// set target altitude to current altitude above home // 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; return target_loc;
} }
@ -1065,9 +1064,9 @@ void Copter::ModeAuto::do_takeoff(const AP_Mission::Mission_Command& cmd)
takeoff_start(cmd.content.location); 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 // use current lat, lon if zero
if (ret.lat == 0 && ret.lng == 0) { 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 // do_nav_wp - initiate move to next waypoint
void Copter::ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) 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. // this will be used to remember the time in millis after we reach or pass the WP.
loiter_time = 0; loiter_time = 0;
@ -1119,7 +1118,7 @@ void Copter::ModeAuto::do_land(const AP_Mission::Mission_Command& cmd)
// set state to fly to location // set state to fly to location
land_state = LandStateType_FlyToLocation; land_state = LandStateType_FlyToLocation;
Location_Class target_loc = terrain_adjusted_location(cmd); const Location target_loc = terrain_adjusted_location(cmd);
wp_start(target_loc); wp_start(target_loc);
} else { } 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) void Copter::ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{ {
// convert back to location // convert back to location
Location_Class target_loc(cmd.content.location); Location target_loc(cmd.content.location);
// use current location if not provided // use current location if not provided
if (target_loc.lat == 0 && target_loc.lng == 0) { if (target_loc.lat == 0 && target_loc.lng == 0) {
// To-Do: make this simpler // To-Do: make this simpler
Vector3f temp_pos; Vector3f temp_pos;
copter.wp_nav->get_wp_stopping_point_xy(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.lat = temp_loc.lat;
target_loc.lng = temp_loc.lng; 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 // do_circle - initiate moving in a circle
void Copter::ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd) 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 // calculate radius
uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1 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 // if we aren't navigating to a location then we have to adjust
// altitude for current location // altitude for current location
Location_Class target_loc(cmd.content.location); Location target_loc(cmd.content.location);
const Location_Class &current_loc = copter.current_loc;
if (target_loc.lat == 0 && target_loc.lng == 0) { if (target_loc.lat == 0 && target_loc.lng == 0) {
target_loc.lat = current_loc.lat; target_loc.lat = copter.current_loc.lat;
target_loc.lng = current_loc.lng; 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_destination_xy = true;
loiter_to_alt.reached_alt = true; loiter_to_alt.reached_alt = true;
gcs().send_text(MAV_SEVERITY_INFO, "bad do_loiter_to_alt"); 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 // do_spline_wp - initiate move to next waypoint
void Copter::ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd) 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. // this will be used to remember the time in millis after we reach or pass the WP.
loiter_time = 0; 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 // 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)) { if (cmd.p1 == 0 && mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
next_loc = temp_cmd.content.location; next_loc = temp_cmd.content.location;
// default lat, lon to first waypoint's lat, lon // 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 // set state to fly to location
nav_payload_place.state = PayloadPlaceStateType_FlyToLocation; 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); wp_start(target_loc);
} else { } else {
@ -1654,7 +1652,7 @@ bool Copter::ModeAuto::verify_payload_place()
} }
FALLTHROUGH; FALLTHROUGH;
case PayloadPlaceStateType_Ascending_Start: { 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; target_loc.alt = nav_payload_place.descend_start_altitude;
wp_start(target_loc); wp_start(target_loc);
nav_payload_place.state = PayloadPlaceStateType_Ascending; nav_payload_place.state = PayloadPlaceStateType_Ascending;

2
ArduCopter/mode_follow.cpp
View File

@ -163,7 +163,7 @@ int32_t Copter::ModeFollow::wp_bearing() const
/* /*
get target position for mavlink reporting 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 dist = g2.follow.get_distance_to_target();
float bearing = g2.follow.get_bearing_to_target(); float bearing = g2.follow.get_bearing_to_target();

12
ArduCopter/mode_guided.cpp
View File

@ -54,8 +54,8 @@ bool Copter::ModeGuided::do_user_takeoff_start(float final_alt_above_home)
guided_mode = Guided_TakeOff; guided_mode = Guided_TakeOff;
// initialise wpnav destination // initialise wpnav destination
Location_Class target_loc = copter.current_loc; Location target_loc = copter.current_loc;
target_loc.set_alt_cm(final_alt_above_home, Location_Class::ALT_FRAME_ABOVE_HOME); target_loc.set_alt_cm(final_alt_above_home, Location::ALT_FRAME_ABOVE_HOME);
if (!wp_nav->set_wp_destination(target_loc)) { if (!wp_nav->set_wp_destination(target_loc)) {
// failure to set destination can only be because of missing terrain data // 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 #if AC_FENCE == ENABLED
// reject destination if outside the fence // 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)) { if (!copter.fence.check_destination_within_fence(dest_loc)) {
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE); copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK // failure is propagated to GCS with NAK
@ -201,7 +201,7 @@ bool Copter::ModeGuided::set_destination(const Vector3f& destination, bool use_y
return true; return true;
} }
bool Copter::ModeGuided::get_wp(Location_Class& destination) bool Copter::ModeGuided::get_wp(Location& destination)
{ {
if (guided_mode != Guided_WP) { if (guided_mode != Guided_WP) {
return false; return false;
@ -212,7 +212,7 @@ bool Copter::ModeGuided::get_wp(Location_Class& destination)
// sets guided mode's target from a Location object // sets guided mode's target from a Location object
// returns false if destination could not be set (probably caused by missing terrain data) // 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 // 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 // ensure we are in position control mode
if (guided_mode != Guided_WP) { if (guided_mode != Guided_WP) {
@ -275,7 +275,7 @@ bool Copter::ModeGuided::set_destination_posvel(const Vector3f& destination, con
#if AC_FENCE == ENABLED #if AC_FENCE == ENABLED
// reject destination if outside the fence // 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)) { if (!copter.fence.check_destination_within_fence(dest_loc)) {
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE); copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK // failure is propagated to GCS with NAK

24
ArduCopter/mode_rtl.cpp
View File

@ -389,17 +389,17 @@ void Copter::ModeRTL::build_path(bool terrain_following_allowed)
Vector3f stopping_point; Vector3f stopping_point;
pos_control->get_stopping_point_xy(stopping_point); pos_control->get_stopping_point_xy(stopping_point);
pos_control->get_stopping_point_z(stopping_point); pos_control->get_stopping_point_z(stopping_point);
rtl_path.origin_point = Location_Class(stopping_point); rtl_path.origin_point = Location(stopping_point);
rtl_path.origin_point.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME); rtl_path.origin_point.change_alt_frame(Location::ALT_FRAME_ABOVE_HOME);
// compute return target // compute return target
compute_return_target(terrain_following_allowed); compute_return_target(terrain_following_allowed);
// climb target is above our origin point at the return altitude // 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 // 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 // set land flag
rtl_path.land = g.rtl_alt_final <= 0; 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) { if (rtl_path.terrain_used) {
// attempt to retrieve terrain alt for current location, stopping point and origin // attempt to retrieve terrain alt for current location, stopping point and origin
int32_t origin_terr_alt, return_target_terr_alt; 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) || if (!rtl_path.origin_point.get_alt_cm(Location::ALT_FRAME_ABOVE_TERRAIN, origin_terr_alt) ||
!rtl_path.return_target.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, return_target_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_Class::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) { !copter.current_loc.get_alt_cm(Location::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) {
rtl_path.terrain_used = false; rtl_path.terrain_used = false;
copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); 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 // 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.terrain_used || !rtl_path.return_target.change_alt_frame(Location::ALT_FRAME_ABOVE_TERRAIN)) {
if (!rtl_path.return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME)) { if (!rtl_path.return_target.change_alt_frame(Location::ALT_FRAME_ABOVE_HOME)) {
// this should never happen but just in case // 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; 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 // 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 #if AC_FENCE == ENABLED
// ensure not above fence altitude if alt fence is 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 // 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) { 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 // 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; float fence_alt = copter.fence.get_safe_alt_max()*100.0f;
if (target_alt > fence_alt) { if (target_alt > fence_alt) {
// reduce target alt to the fence alt // reduce target alt to the fence alt

2
ArduCopter/precision_landing.cpp
View File

@ -19,7 +19,7 @@ void Copter::update_precland()
if (rangefinder_alt_ok()) { if (rangefinder_alt_ok()) {
height_above_ground_cm = rangefinder_state.alt_cm; height_above_ground_cm = rangefinder_state.alt_cm;
} else if (terrain_use()) { } 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; height_above_ground_cm = current_loc.alt;
} }
} }

2
ArduCopter/system.cpp
View File

@ -161,7 +161,7 @@ void Copter::init_ardupilot()
// init Location class // init Location class
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN #if AP_TERRAIN_AVAILABLE && AC_TERRAIN
Location_Class::set_terrain(&terrain); Location::set_terrain(&terrain);
wp_nav->set_terrain(&terrain); wp_nav->set_terrain(&terrain);
#endif #endif