ArduPilot/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2025-12-11 02:29:09 -06:00
Code Issues Packages Projects Releases 82 Wiki Activity

Sub: adjust for Location_Class and Location unification

Browse Source
This commit is contained in:
Peter Barker 2019-01-02 15:40:53 +11:00 committed by Peter Barker
parent 845f015648
commit b4c65dde92
5 changed files with 32 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduSub/Sub.h
View File

@ -347,7 +347,7 @@ private:
// 3D Location vectors
// Current location of the Sub (altitude is relative to home)
Location_Class current_loc;
Location current_loc;
// Navigation Yaw control
// auto flight mode's yaw mode
@ -522,10 +522,10 @@ private:
bool auto_init(void);
void auto_run();
void auto_wp_start(const Vector3f& destination);
void auto_wp_start(const Location_Class& dest_loc);
void auto_wp_start(const Location& dest_loc);
void auto_wp_run();
void auto_spline_run();
void auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m);
void auto_circle_movetoedge_start(const Location &circle_center, float radius_m);
void auto_circle_start();
void auto_circle_run();
void auto_nav_guided_start();
@ -545,7 +545,7 @@ private:
void guided_posvel_control_start();
void guided_angle_control_start();
bool guided_set_destination(const Vector3f& destination);
bool guided_set_destination(const Location_Class& dest_loc);
bool guided_set_destination(const Location& dest_loc);
void guided_set_velocity(const Vector3f& velocity);
bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity);
void guided_set_angle(const Quaternion &q, float climb_rate_cms);
@ -658,7 +658,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);
void log_init(void);
void init_capabilities(void);
void accel_cal_update(void);

38
ArduSub/commands_logic.cpp
View File

@ -12,7 +12,7 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
DataFlash.Log_Write_Mission_Cmd(mission, cmd);
}
Location_Class target_loc(cmd.content.location);
const Location &target_loc = cmd.content.location;
// target alt must be negative (underwater)
if (target_loc.alt > 0.0f) {
@ -21,7 +21,7 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
}
// only tested/supported alt frame so far is ALT_FRAME_ABOVE_HOME, where Home alt is always water's surface ie zero depth
if (target_loc.get_alt_frame() != Location_Class::ALT_FRAME_ABOVE_HOME) {
if (target_loc.get_alt_frame() != Location::ALT_FRAME_ABOVE_HOME) {
gcs().send_text(MAV_SEVERITY_WARNING, "BAD NAV ALT_FRAME %d", (int8_t)target_loc.get_alt_frame());
return false;
}
@ -227,7 +227,7 @@ void Sub::exit_mission()
// do_nav_wp - initiate move to next waypoint
void Sub::do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
Location_Class target_loc(cmd.content.location);
Location target_loc(cmd.content.location);
// use current lat, lon if zero
if (target_loc.lat == 0 && target_loc.lng == 0) {
target_loc.lat = current_loc.lat;
@ -262,7 +262,7 @@ void Sub::do_nav_wp(const AP_Mission::Mission_Command& cmd)
// do_surface - initiate surface procedure
void Sub::do_surface(const AP_Mission::Mission_Command& cmd)
{
Location_Class target_location;
Location target_location;
// if location provided we fly to that location at current altitude
if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
@ -271,24 +271,24 @@ void Sub::do_surface(const AP_Mission::Mission_Command& cmd)
// calculate and set desired location below surface target
// convert to location class
target_location = Location_Class(cmd.content.location);
target_location = Location(cmd.content.location);
// decide if we will use terrain following
int32_t curr_terr_alt_cm, target_terr_alt_cm;
if (current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) &&
target_location.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) {
if (current_loc.get_alt_cm(Location::ALT_FRAME_ABOVE_TERRAIN, curr_terr_alt_cm) &&
target_location.get_alt_cm(Location::ALT_FRAME_ABOVE_TERRAIN, target_terr_alt_cm)) {
// if using terrain, set target altitude to current altitude above terrain
target_location.set_alt_cm(curr_terr_alt_cm, Location_Class::ALT_FRAME_ABOVE_TERRAIN);
target_location.set_alt_cm(curr_terr_alt_cm, Location::ALT_FRAME_ABOVE_TERRAIN);
} else {
// set target altitude to current altitude above home
target_location.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
target_location.set_alt_cm(current_loc.alt, Location::ALT_FRAME_ABOVE_HOME);
}
} else {
// set surface state to ascend
auto_surface_state = AUTO_SURFACE_STATE_ASCEND;
// Set waypoint destination to current location at zero depth
target_location = Location_Class(current_loc.lat, current_loc.lng, 0, Location_Class::ALT_FRAME_ABOVE_HOME);
target_location = Location(current_loc.lat, current_loc.lng, 0, Location::ALT_FRAME_ABOVE_HOME);
}
// Go to wp location
@ -305,14 +305,14 @@ void Sub::do_RTL()
void Sub::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;
wp_nav.get_wp_stopping_point_xy(temp_pos);
Location_Class temp_loc(temp_pos);
Location temp_loc(temp_pos);
target_loc.lat = temp_loc.lat;
target_loc.lng = temp_loc.lng;
}
@ -342,7 +342,7 @@ void Sub::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
// do_circle - initiate moving in a circle
void Sub::do_circle(const AP_Mission::Mission_Command& cmd)
{
Location_Class circle_center(cmd.content.location);
Location circle_center(cmd.content.location);
// default lat/lon to current position if not provided
// To-Do: use stopping point or position_controller's target instead of current location to avoid jerk?
@ -386,7 +386,7 @@ void Sub::do_loiter_time(const AP_Mission::Mission_Command& cmd)
// do_spline_wp - initiate move to next waypoint
void Sub::do_spline_wp(const AP_Mission::Mission_Command& cmd)
{
Location_Class target_loc(cmd.content.location);
Location target_loc(cmd.content.location);
// use current lat, lon if zero
if (target_loc.lat == 0 && target_loc.lng == 0) {
target_loc.lat = current_loc.lat;
@ -426,7 +426,7 @@ void Sub::do_spline_wp(const AP_Mission::Mission_Command& cmd)
}
// if there is no delay at the end of this segment get next nav command
Location_Class next_loc;
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
@ -536,7 +536,7 @@ bool Sub::verify_surface(const AP_Mission::Mission_Command& cmd)
if (wp_nav.reached_wp_destination()) {
// Set target to current xy and zero depth
// TODO get xy target from current wp destination, because current location may be acceptance-radius away from original destination
Location_Class target_location(cmd.content.location.lat, cmd.content.location.lng, 0, Location_Class::ALT_FRAME_ABOVE_HOME);
Location target_location(cmd.content.location.lat, cmd.content.location.lng, 0, Location::ALT_FRAME_ABOVE_HOME);
auto_wp_start(target_location);
@ -740,20 +740,16 @@ bool Sub::do_guided(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_NAV_WAYPOINT: {
// set wp_nav's destination
Location_Class dest(cmd.content.location);
return guided_set_destination(dest);
break;
return guided_set_destination(cmd.content.location);
}
case MAV_CMD_CONDITION_YAW:
do_yaw(cmd);
return true;
break;
default:
// reject unrecognised command
return false;
break;
}
return true;

12
ArduSub/control_auto.cpp
View File

@ -89,7 +89,7 @@ void Sub::auto_wp_start(const Vector3f& destination)
}
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
void Sub::auto_wp_start(const Location_Class& dest_loc)
void Sub::auto_wp_start(const Location& dest_loc)
{
auto_mode = Auto_WP;
@ -174,9 +174,9 @@ void Sub::auto_wp_run()
// 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 Sub::auto_spline_start(const Location_Class& destination, bool stopped_at_start,
void Sub::auto_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)
{
auto_mode = Auto_Spline;
@ -252,7 +252,7 @@ void Sub::auto_spline_run()
// 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 set the circle's circle with circle_nav.set_center()
// we assume the caller has performed all required GPS_ok checks
void Sub::auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m)
void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radius_m)
{
// convert location to vector from ekf origin
Vector3f circle_center_neu;
@ -278,8 +278,8 @@ void Sub::auto_circle_movetoedge_start(const Location_Class &circle_center, floa
// set the state to move to the edge of the circle
auto_mode = Auto_CircleMoveToEdge;
// convert circle_edge_neu to Location_Class
Location_Class circle_edge(circle_edge_neu);
// convert 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());

6
ArduSub/control_guided.cpp
View File

@ -147,7 +147,7 @@ bool Sub::guided_set_destination(const Vector3f& destination)
#if AC_FENCE == ENABLED
// reject destination if outside the fence
Location_Class dest_loc(destination);
const Location dest_loc(destination);
if (!fence.check_destination_within_fence(dest_loc)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
@ -166,7 +166,7 @@ bool Sub::guided_set_destination(const Vector3f& 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 Sub::guided_set_destination(const Location_Class& dest_loc)
bool Sub::guided_set_destination(const Location& dest_loc)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
@ -219,7 +219,7 @@ bool Sub::guided_set_destination_posvel(const Vector3f& destination, const Vecto
#if AC_FENCE == ENABLED
// reject destination if outside the fence
Location_Class dest_loc(destination);
const Location dest_loc(destination);
if (!fence.check_destination_within_fence(dest_loc)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK

2
ArduSub/system.cpp
View File

@ -117,7 +117,7 @@ void Sub::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