Skip to content

Commit

Permalink
All squashed
Browse files Browse the repository at this point in the history
  • Loading branch information
@lthall
lthall committed Sep 15, 2024
1 parent 66a2788 commit d60a5ee
Show file tree
Hide file tree
Showing 20 changed files with 903 additions and 92 deletions.
38 changes: 38 additions & 0 deletions ArduCopter/Copter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -413,6 +413,44 @@ bool Copter::set_desired_speed(float speed)
}

#if MODE_AUTO_ENABLED
// add an additional offset to vehicle's target position, velocity and acceleration
// units are m, m/s and m/s/s in NED frame
// Z-axis is not currently supported and is ignored
bool Copter::set_posvelaccel_offset(const Vector3f &pos_offset_NED, const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED)
{
// only supported in Auto mode
if (flightmode != &mode_auto) {
return false;
}

pos_control->set_posvelaccel_offset_target_xy_cm(pos_offset_NED.topostype().xy() * 100.0, vel_offset_NED.xy() * 100.0, accel_offset_NED.xy() * 100.0);
return true;
}

// add an additional offset to vehicle's target velocity and acceleration
// units are m/s and m/s/s in NED frame
// Z-axis is not currently supported and is ignored
bool Copter::set_velaccel_offset(const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED)
{
// only supported in Auto mode
if (flightmode != &mode_auto) {
return false;
}

pos_control->set_velaccel_offset_target_xy_cms(vel_offset_NED.xy() * 100.0, accel_offset_NED.xy() * 100.0);
return true;
}

// get position and velocity offset to vehicle's target velocity and acceleration
// units are m and m/s in NED frame
bool Copter::get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &vel_offset_NED, Vector3f &accel_offset_NED)
{
pos_offset_NED = pos_control->get_pos_offset_cm().tofloat() * 0.01;
vel_offset_NED = pos_control->get_vel_offset_cms() * 0.01;
accel_offset_NED = pos_control->get_accel_offset_cmss() * 0.01;
return true;
}

// returns true if mode supports NAV_SCRIPT_TIME mission commands
bool Copter::nav_scripting_enable(uint8_t mode)
{
Expand Down
3 changes: 3 additions & 0 deletions ArduCopter/Copter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -688,6 +688,9 @@ class Copter : public AP_Vehicle {
#endif
bool set_desired_speed(float speed) override;
#if MODE_AUTO_ENABLED
bool set_posvelaccel_offset(const Vector3f &pos_offset_NED, const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED) override;
bool set_velaccel_offset(const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED) override;
bool get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &vel_offset_NED, Vector3f &accel_offset_NED) override;
bool nav_scripting_enable(uint8_t mode) override;
bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) override;
void nav_script_time_done(uint16_t id) override;
Expand Down
4 changes: 4 additions & 0 deletions ArduCopter/mode.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -631,6 +631,10 @@ class ModeAuto : public Mode {

bool shift_alt_to_current_alt(Location& target_loc) const;

// subtract position controller offsets from target location
// should be used when the location will be used as a target for the position controller
void subtract_pos_offsets(Location& target_loc) const;

void do_takeoff(const AP_Mission::Mission_Command& cmd);
void do_nav_wp(const AP_Mission::Mission_Command& cmd);
bool set_next_wp(const AP_Mission::Mission_Command& current_cmd, const Location &default_loc);
Expand Down
66 changes: 39 additions & 27 deletions ArduCopter/mode_auto.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -498,7 +498,7 @@ void ModeAuto::land_start()
set_submode(SubMode::LAND);
}

// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
// 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 ModeAuto::circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn)
{
Expand All @@ -517,8 +517,8 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi

// check our distance from edge of circle
Vector3f circle_edge_neu;
copter.circle_nav->get_closest_point_on_circle(circle_edge_neu);
float dist_to_edge = (inertial_nav.get_position_neu_cm() - circle_edge_neu).length();
float dist_to_edge;
copter.circle_nav->get_closest_point_on_circle(circle_edge_neu, dist_to_edge);

// if more than 3m then fly to edge
if (dist_to_edge > 300.0f) {
Expand Down Expand Up @@ -1488,6 +1488,14 @@ bool ModeAuto::shift_alt_to_current_alt(Location& target_loc) const
return true;
}

// subtract position controller offsets from target location
// should be used when the location will be used as a target for the position controller
void ModeAuto::subtract_pos_offsets(Location& target_loc) const
{
// subtract position controller offsets from target location
target_loc.offset(-pos_control->get_pos_offset_cm() * 0.01);
}

/********************************************************************************/
// Nav (Must) commands
/********************************************************************************/
Expand Down Expand Up @@ -1528,6 +1536,10 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
// calculate default location used when lat, lon or alt is zero
Location default_loc = copter.current_loc;

// subtract position offsets
subtract_pos_offsets(default_loc);

if (wp_nav->is_active() && wp_nav->reached_wp_destination()) {
if (!wp_nav->get_wp_destination_loc(default_loc)) {
// this should never happen
Expand Down Expand Up @@ -1648,33 +1660,23 @@ void ModeAuto::do_land(const AP_Mission::Mission_Command& cmd)
// note: caller should set yaw_mode
void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
// convert back to location
Location target_loc(cmd.content.location);
// calculate default location used when lat, lon or alt is zero
Location default_loc = copter.current_loc;

// 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.xy());
const Location temp_loc(temp_pos, Location::AltFrame::ABOVE_ORIGIN);
target_loc.lat = temp_loc.lat;
target_loc.lng = temp_loc.lng;
}

// use current altitude if not provided
// To-Do: use z-axis stopping point instead of current alt
if (target_loc.alt == 0) {
// set to current altitude but in command's alt frame
int32_t curr_alt;
if (copter.current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) {
target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame());
} else {
// default to current altitude as alt-above-home
target_loc.set_alt_cm(copter.current_loc.alt,
copter.current_loc.get_alt_frame());
// subtract position offsets
subtract_pos_offsets(default_loc);

// use previous waypoint destination as default if available
if (wp_nav->is_active() && wp_nav->reached_wp_destination()) {
if (!wp_nav->get_wp_destination_loc(default_loc)) {
// this should never happen
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
}

// get waypoint's location from command and send to wp_nav
const Location target_loc = loc_from_cmd(cmd, default_loc);

// start way point navigator and provide it the desired location
if (!wp_start(target_loc)) {
// failure to set next destination can only be because of missing terrain data
Expand All @@ -1686,7 +1688,13 @@ void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
// do_circle - initiate moving in a circle
void ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
{
const Location circle_center = loc_from_cmd(cmd, copter.current_loc);
// calculate default location used when lat, lon or alt is zero
Location default_loc = copter.current_loc;

// subtract position offsets
subtract_pos_offsets(default_loc);

const Location circle_center = loc_from_cmd(cmd, default_loc);

// calculate radius
uint16_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
Expand Down Expand Up @@ -1754,6 +1762,10 @@ void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
{
// calculate default location used when lat, lon or alt is zero
Location default_loc = copter.current_loc;

// subtract position offsets
subtract_pos_offsets(default_loc);

if (wp_nav->is_active() && wp_nav->reached_wp_destination()) {
if (!wp_nav->get_wp_destination_loc(default_loc)) {
// this should never happen
Expand Down
4 changes: 2 additions & 2 deletions ArduSub/mode_auto.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -187,8 +187,8 @@ void ModeAuto::auto_circle_movetoedge_start(const Location &circle_center, float

// check our distance from edge of circle
Vector3f circle_edge_neu;
sub.circle_nav.get_closest_point_on_circle(circle_edge_neu);
float dist_to_edge = (inertial_nav.get_position_neu_cm() - circle_edge_neu).length();
float dist_to_edge;
sub.circle_nav.get_closest_point_on_circle(circle_edge_neu, dist_to_edge);

// if more than 3m then fly to edge
if (dist_to_edge > 300.0f) {
Expand Down
Loading

0 comments on commit d60a5ee

Please sign in to comment.