mirror of https://github.com/ArduPilot/ardupilot
Copter: adjust for Location_Class and Location unification
This commit is contained in:
Peter Barker
Peter Barker
parent
4e94d5c0ce
commit
845f015648
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue