mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
Copter: FlightMode - convert GUIDED flight mode
This commit is contained in:
Peter Barker
Randy Mackay
parent
f2495b2d08
commit
2db09ba0f7
@ -33,7 +33,6 @@ Copter::Copter(void)
|
||||
home_bearing(0),
|
||||
home_distance(0),
|
||||
wp_distance(0),
|
||||
guided_mode(Guided_TakeOff),
|
||||
rtl_state(RTL_InitialClimb),
|
||||
rtl_state_complete(false),
|
||||
smart_rtl_state(SmartRTL_PathFollow),
|
||||
|
||||
@ -392,9 +392,6 @@ private:
|
||||
float descend_max; // centimetres
|
||||
} nav_payload_place;
|
||||
|
||||
// Guided
|
||||
GuidedMode guided_mode; // controls which controller is run (pos or vel)
|
||||
|
||||
// RTL
|
||||
RTLState rtl_state; // records state of rtl (initial climb, returning home, etc)
|
||||
bool rtl_state_complete; // set to true if the current state is completed
|
||||
@ -841,31 +838,8 @@ private:
|
||||
float get_throttle_assist(float velz, float pilot_throttle_scaled);
|
||||
bool flip_init(bool ignore_checks);
|
||||
void flip_run();
|
||||
bool guided_init(bool ignore_checks);
|
||||
bool guided_takeoff_start(float final_alt_above_home);
|
||||
void guided_pos_control_start();
|
||||
void guided_vel_control_start();
|
||||
void guided_posvel_control_start();
|
||||
void guided_angle_control_start();
|
||||
bool guided_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 guided_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);
|
||||
void guided_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 guided_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);
|
||||
void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
|
||||
void guided_run();
|
||||
void guided_takeoff_run();
|
||||
void guided_pos_control_run();
|
||||
void guided_vel_control_run();
|
||||
void guided_posvel_control_run();
|
||||
void guided_angle_control_run();
|
||||
void guided_set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des);
|
||||
void guided_limit_clear();
|
||||
void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);
|
||||
void guided_limit_init_time_and_pos();
|
||||
bool guided_limit_check();
|
||||
bool guided_nogps_init(bool ignore_checks);
|
||||
void guided_nogps_run();
|
||||
void guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);
|
||||
bool land_init(bool ignore_checks);
|
||||
void land_run();
|
||||
void land_gps_run();
|
||||
@ -1128,6 +1102,8 @@ private:
|
||||
|
||||
Copter::FlightMode_CIRCLE flightmode_circle{*this, circle_nav};
|
||||
|
||||
Copter::FlightMode_GUIDED flightmode_guided{*this};
|
||||
|
||||
Copter::FlightMode_LOITER flightmode_loiter{*this};
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
|
||||
@ -413,3 +413,65 @@ private:
|
||||
#endif
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
class FlightMode_GUIDED : public FlightMode {
|
||||
|
||||
public:
|
||||
|
||||
FlightMode_GUIDED(Copter &copter) :
|
||||
Copter::FlightMode(copter) { }
|
||||
|
||||
bool init(bool ignore_checks) override;
|
||||
void run() override; // should be called at 100hz or more
|
||||
|
||||
bool requires_GPS() const override { return true; }
|
||||
bool has_manual_throttle() const override { return false; }
|
||||
bool allows_arming(bool from_gcs) const override {
|
||||
if (from_gcs) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
};
|
||||
bool is_autopilot() const override { return true; }
|
||||
|
||||
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);
|
||||
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 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);
|
||||
|
||||
void limit_clear();
|
||||
void limit_init_time_and_pos();
|
||||
void limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);
|
||||
bool limit_check();
|
||||
|
||||
bool takeoff_start(float final_alt_above_home);
|
||||
|
||||
GuidedMode mode() { return guided_mode; }
|
||||
|
||||
void angle_control_start();
|
||||
void angle_control_run();
|
||||
|
||||
protected:
|
||||
|
||||
const char *name() const override { return "GUIDED"; }
|
||||
const char *name4() const override { return "GUID"; }
|
||||
|
||||
private:
|
||||
|
||||
void pos_control_start();
|
||||
void vel_control_start();
|
||||
void posvel_control_start();
|
||||
void takeoff_run();
|
||||
void pos_control_run();
|
||||
void vel_control_run();
|
||||
void posvel_control_run();
|
||||
void set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des);
|
||||
void set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);
|
||||
|
||||
// controls which controller is run (pos or vel):
|
||||
GuidedMode guided_mode = Guided_TakeOff;
|
||||
|
||||
};
|
||||
|
||||
@ -1364,7 +1364,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
|
||||
use_yaw_rate = true;
|
||||
}
|
||||
|
||||
copter.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]),
|
||||
copter.flightmode_guided.set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]),
|
||||
climb_rate_cms, use_yaw_rate, packet.body_yaw_rate);
|
||||
|
||||
break;
|
||||
@ -1446,16 +1446,16 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
|
||||
|
||||
// send request
|
||||
if (!pos_ignore && !vel_ignore && acc_ignore) {
|
||||
if (copter.guided_set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
|
||||
if (copter.flightmode_guided.set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
} else {
|
||||
result = MAV_RESULT_FAILED;
|
||||
}
|
||||
} else if (pos_ignore && !vel_ignore && acc_ignore) {
|
||||
copter.guided_set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
|
||||
copter.flightmode_guided.set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
} else if (!pos_ignore && vel_ignore && acc_ignore) {
|
||||
if (copter.guided_set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
|
||||
if (copter.flightmode_guided.set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
} else {
|
||||
result = MAV_RESULT_FAILED;
|
||||
@ -1548,16 +1548,16 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
|
||||
}
|
||||
|
||||
if (!pos_ignore && !vel_ignore && acc_ignore) {
|
||||
if (copter.guided_set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
|
||||
if (copter.flightmode_guided.set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
} else {
|
||||
result = MAV_RESULT_FAILED;
|
||||
}
|
||||
} else if (pos_ignore && !vel_ignore && acc_ignore) {
|
||||
copter.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
|
||||
copter.flightmode_guided.set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
} else if (!pos_ignore && vel_ignore && acc_ignore) {
|
||||
if (copter.guided_set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
|
||||
if (copter.flightmode_guided.set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
} else {
|
||||
result = MAV_RESULT_FAILED;
|
||||
|
||||
@ -578,7 +578,7 @@ void Copter::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
if (cmd.p1 > 0) {
|
||||
// initialise guided limits
|
||||
guided_limit_init_time_and_pos();
|
||||
flightmode_guided.limit_init_time_and_pos();
|
||||
|
||||
// set spline navigation target
|
||||
flightmode_auto.nav_guided_start();
|
||||
@ -649,7 +649,7 @@ void Copter::do_gripper(const AP_Mission::Mission_Command& cmd)
|
||||
// do_guided_limits - pass guided limits to guided controller
|
||||
void Copter::do_guided_limits(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
guided_limit_set(cmd.p1 * 1000, // convert seconds to ms
|
||||
flightmode_guided.limit_set(cmd.p1 * 1000, // convert seconds to ms
|
||||
cmd.content.guided_limits.alt_min * 100.0f, // convert meters to cm
|
||||
cmd.content.guided_limits.alt_max * 100.0f, // convert meters to cm
|
||||
cmd.content.guided_limits.horiz_max * 100.0f); // convert meters to cm
|
||||
@ -1005,7 +1005,7 @@ bool Copter::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
|
||||
}
|
||||
|
||||
// check time and position limits
|
||||
return guided_limit_check();
|
||||
return flightmode_guided.limit_check();
|
||||
}
|
||||
#endif // NAV_GUIDED
|
||||
|
||||
@ -1104,7 +1104,7 @@ bool Copter::do_guided(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// set wp_nav's destination
|
||||
Location_Class dest(cmd.content.location);
|
||||
return guided_set_destination(dest);
|
||||
return flightmode_guided.set_destination(dest);
|
||||
}
|
||||
|
||||
case MAV_CMD_CONDITION_YAW:
|
||||
|
||||
@ -39,7 +39,7 @@ bool Copter::FlightMode_AUTO::init(bool ignore_checks)
|
||||
wp_nav->wp_and_spline_init();
|
||||
|
||||
// clear guided limits
|
||||
_copter.guided_limit_clear();
|
||||
_copter.flightmode_guided.limit_clear();
|
||||
|
||||
// start/resume the mission (based on MIS_RESTART parameter)
|
||||
mission.start_or_resume();
|
||||
@ -534,10 +534,10 @@ void Copter::FlightMode_AUTO::nav_guided_start()
|
||||
_mode = Auto_NavGuided;
|
||||
|
||||
// call regular guided flight mode initialisation
|
||||
_copter.guided_init(true);
|
||||
_copter.flightmode_guided.init(true);
|
||||
|
||||
// initialise guided start time and position as reference for limit checking
|
||||
_copter.guided_limit_init_time_and_pos();
|
||||
_copter.flightmode_guided.limit_init_time_and_pos();
|
||||
}
|
||||
|
||||
// auto_nav_guided_run - allows control by external navigation controller
|
||||
@ -545,7 +545,7 @@ void Copter::FlightMode_AUTO::nav_guided_start()
|
||||
void Copter::FlightMode_AUTO::nav_guided_run()
|
||||
{
|
||||
// call regular guided flight mode run function
|
||||
_copter.guided_run();
|
||||
_copter.flightmode_guided.run();
|
||||
}
|
||||
#endif // NAV_GUIDED
|
||||
|
||||
|
||||
@ -13,7 +13,7 @@
|
||||
bool Copter::avoid_adsb_init(const bool ignore_checks)
|
||||
{
|
||||
// re-use guided mode
|
||||
return guided_init(ignore_checks);
|
||||
return flightmode_guided.init(ignore_checks);
|
||||
}
|
||||
|
||||
bool Copter::avoid_adsb_set_velocity(const Vector3f& velocity_neu)
|
||||
@ -24,7 +24,7 @@ bool Copter::avoid_adsb_set_velocity(const Vector3f& velocity_neu)
|
||||
}
|
||||
|
||||
// re-use guided mode's velocity controller
|
||||
guided_set_velocity(velocity_neu);
|
||||
flightmode_guided.set_velocity(velocity_neu);
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -34,5 +34,5 @@ void Copter::avoid_adsb_run()
|
||||
// re-use guided mode's velocity controller
|
||||
// Note: this is safe from interference from GCSs and companion computer's whose guided mode
|
||||
// position and velocity requests will be ignored while the vehicle is not in guided mode
|
||||
guided_run();
|
||||
flightmode_guided.run();
|
||||
}
|
||||
|
||||
@ -36,13 +36,13 @@ struct Guided_Limit {
|
||||
} guided_limit;
|
||||
|
||||
// guided_init - initialise guided controller
|
||||
bool Copter::guided_init(bool ignore_checks)
|
||||
bool Copter::FlightMode_GUIDED::init(bool ignore_checks)
|
||||
{
|
||||
if (position_ok() || ignore_checks) {
|
||||
if (_copter.position_ok() || ignore_checks) {
|
||||
// initialise yaw
|
||||
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
|
||||
set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false));
|
||||
// start in position control mode
|
||||
guided_pos_control_start();
|
||||
pos_control_start();
|
||||
return true;
|
||||
}else{
|
||||
return false;
|
||||
@ -51,17 +51,17 @@ bool Copter::guided_init(bool ignore_checks)
|
||||
|
||||
|
||||
// guided_takeoff_start - initialises waypoint controller to implement take-off
|
||||
bool Copter::guided_takeoff_start(float final_alt_above_home)
|
||||
bool Copter::FlightMode_GUIDED::takeoff_start(float final_alt_above_home)
|
||||
{
|
||||
guided_mode = Guided_TakeOff;
|
||||
|
||||
// initialise wpnav destination
|
||||
Location_Class target_loc = current_loc;
|
||||
Location_Class target_loc = _copter.current_loc;
|
||||
target_loc.set_alt_cm(final_alt_above_home, Location_Class::ALT_FRAME_ABOVE_HOME);
|
||||
|
||||
if (!wp_nav->set_wp_destination(target_loc)) {
|
||||
// failure to set destination can only be because of missing terrain data
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
|
||||
_copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
|
||||
// failure is propagated to GCS with NAK
|
||||
return false;
|
||||
}
|
||||
@ -73,13 +73,13 @@ bool Copter::guided_takeoff_start(float final_alt_above_home)
|
||||
set_throttle_takeoff();
|
||||
|
||||
// get initial alt for WP_NAVALT_MIN
|
||||
auto_takeoff_set_start_alt();
|
||||
|
||||
_copter.auto_takeoff_set_start_alt();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// initialise guided mode's position controller
|
||||
void Copter::guided_pos_control_start()
|
||||
void Copter::FlightMode_GUIDED::pos_control_start()
|
||||
{
|
||||
// set to position control mode
|
||||
guided_mode = Guided_WP;
|
||||
@ -95,11 +95,11 @@ void Copter::guided_pos_control_start()
|
||||
wp_nav->set_wp_destination(stopping_point, false);
|
||||
|
||||
// initialise yaw
|
||||
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
|
||||
set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false));
|
||||
}
|
||||
|
||||
// initialise guided mode's velocity controller
|
||||
void Copter::guided_vel_control_start()
|
||||
void Copter::FlightMode_GUIDED::vel_control_start()
|
||||
{
|
||||
// set guided_mode to velocity controller
|
||||
guided_mode = Guided_Velocity;
|
||||
@ -118,7 +118,7 @@ void Copter::guided_vel_control_start()
|
||||
}
|
||||
|
||||
// initialise guided mode's posvel controller
|
||||
void Copter::guided_posvel_control_start()
|
||||
void Copter::FlightMode_GUIDED::posvel_control_start()
|
||||
{
|
||||
// set guided_mode to velocity controller
|
||||
guided_mode = Guided_PosVel;
|
||||
@ -146,7 +146,7 @@ void Copter::guided_posvel_control_start()
|
||||
}
|
||||
|
||||
// initialise guided mode's angle controller
|
||||
void Copter::guided_angle_control_start()
|
||||
void Copter::FlightMode_GUIDED::angle_control_start()
|
||||
{
|
||||
// set guided_mode to velocity controller
|
||||
guided_mode = Guided_Angle;
|
||||
@ -163,7 +163,7 @@ void Copter::guided_angle_control_start()
|
||||
|
||||
// initialise targets
|
||||
guided_angle_state.update_time_ms = millis();
|
||||
guided_angle_state.roll_cd = ahrs.roll_sensor;
|
||||
guided_angle_state.roll_cd = _copter.ahrs.roll_sensor;
|
||||
guided_angle_state.pitch_cd = ahrs.pitch_sensor;
|
||||
guided_angle_state.yaw_cd = ahrs.yaw_sensor;
|
||||
guided_angle_state.climb_rate_cms = 0.0f;
|
||||
@ -177,49 +177,49 @@ void Copter::guided_angle_control_start()
|
||||
// guided_set_destination - sets guided mode's target destination
|
||||
// Returns true if the fence is enabled and guided waypoint is within the fence
|
||||
// else return false if the waypoint is outside the fence
|
||||
bool Copter::guided_set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
|
||||
bool Copter::FlightMode_GUIDED::set_destination(const Vector3f& destination, 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) {
|
||||
guided_pos_control_start();
|
||||
pos_control_start();
|
||||
}
|
||||
|
||||
#if AC_FENCE == ENABLED
|
||||
// reject destination if outside the fence
|
||||
Location_Class dest_loc(destination);
|
||||
if (!fence.check_destination_within_fence(dest_loc)) {
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
|
||||
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
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
// set yaw state
|
||||
guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
||||
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
||||
|
||||
// no need to check return status because terrain data is not used
|
||||
wp_nav->set_wp_destination(destination, false);
|
||||
|
||||
// log target
|
||||
Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
|
||||
_copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
|
||||
return true;
|
||||
}
|
||||
|
||||
// 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::guided_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::FlightMode_GUIDED::set_destination(const Location_Class& 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) {
|
||||
guided_pos_control_start();
|
||||
pos_control_start();
|
||||
}
|
||||
|
||||
#if AC_FENCE == ENABLED
|
||||
// reject destination outside the fence.
|
||||
// Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
|
||||
if (!fence.check_destination_within_fence(dest_loc)) {
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
|
||||
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
|
||||
return false;
|
||||
}
|
||||
@ -227,76 +227,76 @@ bool Copter::guided_set_destination(const Location_Class& dest_loc, bool use_yaw
|
||||
|
||||
if (!wp_nav->set_wp_destination(dest_loc)) {
|
||||
// failure to set destination can only be because of missing terrain data
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
|
||||
_copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
|
||||
// failure is propagated to GCS with NAK
|
||||
return false;
|
||||
}
|
||||
|
||||
// set yaw state
|
||||
guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
||||
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
||||
|
||||
// log target
|
||||
Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
|
||||
_copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
|
||||
return true;
|
||||
}
|
||||
|
||||
// guided_set_velocity - sets guided mode's target velocity
|
||||
void Copter::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
|
||||
void Copter::FlightMode_GUIDED::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
|
||||
{
|
||||
// check we are in velocity control mode
|
||||
if (guided_mode != Guided_Velocity) {
|
||||
guided_vel_control_start();
|
||||
vel_control_start();
|
||||
}
|
||||
|
||||
// set yaw state
|
||||
guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
||||
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
||||
|
||||
// record velocity target
|
||||
guided_vel_target_cms = velocity;
|
||||
vel_update_time_ms = millis();
|
||||
|
||||
// log target
|
||||
Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity);
|
||||
_copter.Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity);
|
||||
}
|
||||
|
||||
// set guided mode posvel target
|
||||
bool Copter::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
|
||||
bool Copter::FlightMode_GUIDED::set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
|
||||
{
|
||||
// check we are in velocity control mode
|
||||
if (guided_mode != Guided_PosVel) {
|
||||
guided_posvel_control_start();
|
||||
posvel_control_start();
|
||||
}
|
||||
|
||||
#if AC_FENCE == ENABLED
|
||||
// reject destination if outside the fence
|
||||
Location_Class dest_loc(destination);
|
||||
if (!fence.check_destination_within_fence(dest_loc)) {
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
|
||||
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
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
// set yaw state
|
||||
guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
||||
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
||||
|
||||
posvel_update_time_ms = millis();
|
||||
guided_pos_target_cm = destination;
|
||||
guided_vel_target_cms = velocity;
|
||||
|
||||
pos_control->set_pos_target(guided_pos_target_cm);
|
||||
_copter.pos_control->set_pos_target(guided_pos_target_cm);
|
||||
|
||||
// log target
|
||||
Log_Write_GuidedTarget(guided_mode, destination, velocity);
|
||||
_copter.Log_Write_GuidedTarget(guided_mode, destination, velocity);
|
||||
return true;
|
||||
}
|
||||
|
||||
// set guided mode angle target
|
||||
void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads)
|
||||
void Copter::FlightMode_GUIDED::set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads)
|
||||
{
|
||||
// check we are in velocity control mode
|
||||
if (guided_mode != Guided_Angle) {
|
||||
guided_angle_control_start();
|
||||
angle_control_start();
|
||||
}
|
||||
|
||||
// convert quaternion to euler angles
|
||||
@ -312,52 +312,52 @@ void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms, bool us
|
||||
|
||||
// interpret positive climb rate as triggering take-off
|
||||
if (motors->armed() && !ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) {
|
||||
set_auto_armed(true);
|
||||
_copter.set_auto_armed(true);
|
||||
}
|
||||
|
||||
// log target
|
||||
Log_Write_GuidedTarget(guided_mode,
|
||||
_copter.Log_Write_GuidedTarget(guided_mode,
|
||||
Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd),
|
||||
Vector3f(0.0f, 0.0f, guided_angle_state.climb_rate_cms));
|
||||
}
|
||||
|
||||
// guided_run - runs the guided controller
|
||||
// should be called at 100hz or more
|
||||
void Copter::guided_run()
|
||||
void Copter::FlightMode_GUIDED::run()
|
||||
{
|
||||
// call the correct auto controller
|
||||
switch (guided_mode) {
|
||||
|
||||
case Guided_TakeOff:
|
||||
// run takeoff controller
|
||||
guided_takeoff_run();
|
||||
takeoff_run();
|
||||
break;
|
||||
|
||||
case Guided_WP:
|
||||
// run position controller
|
||||
guided_pos_control_run();
|
||||
pos_control_run();
|
||||
break;
|
||||
|
||||
case Guided_Velocity:
|
||||
// run velocity controller
|
||||
guided_vel_control_run();
|
||||
vel_control_run();
|
||||
break;
|
||||
|
||||
case Guided_PosVel:
|
||||
// run position-velocity controller
|
||||
guided_posvel_control_run();
|
||||
posvel_control_run();
|
||||
break;
|
||||
|
||||
case Guided_Angle:
|
||||
// run angle controller
|
||||
guided_angle_control_run();
|
||||
angle_control_run();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// guided_takeoff_run - takeoff in guided mode
|
||||
// called by guided_run at 100hz or more
|
||||
void Copter::guided_takeoff_run()
|
||||
void Copter::FlightMode_GUIDED::takeoff_run()
|
||||
{
|
||||
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
|
||||
@ -379,7 +379,7 @@ void Copter::guided_takeoff_run()
|
||||
|
||||
// process pilot's yaw input
|
||||
float target_yaw_rate = 0;
|
||||
if (!failsafe.radio) {
|
||||
if (!_copter.failsafe.radio) {
|
||||
// get pilot's desired yaw rate
|
||||
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
||||
}
|
||||
@ -400,18 +400,18 @@ void Copter::guided_takeoff_run()
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run waypoint controller
|
||||
failsafe_terrain_set_status(wp_nav->update_wpnav());
|
||||
_copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
|
||||
|
||||
// call z-axis position controller (wpnav should have already updated it's alt target)
|
||||
pos_control->update_z_controller();
|
||||
_copter.pos_control->update_z_controller();
|
||||
|
||||
// call attitude controller
|
||||
auto_takeoff_attitude_run(target_yaw_rate);
|
||||
_copter.auto_takeoff_attitude_run(target_yaw_rate);
|
||||
}
|
||||
|
||||
// guided_pos_control_run - runs the guided position controller
|
||||
// called from guided_run
|
||||
void Copter::guided_pos_control_run()
|
||||
void Copter::FlightMode_GUIDED::pos_control_run()
|
||||
{
|
||||
// if not auto armed or motors not enabled set throttle to zero and exit immediately
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
|
||||
@ -429,7 +429,7 @@ void Copter::guided_pos_control_run()
|
||||
|
||||
// process pilot's yaw input
|
||||
float target_yaw_rate = 0;
|
||||
if (!failsafe.radio) {
|
||||
if (!_copter.failsafe.radio) {
|
||||
// get pilot's desired yaw rate
|
||||
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
||||
if (!is_zero(target_yaw_rate)) {
|
||||
@ -441,13 +441,13 @@ void Copter::guided_pos_control_run()
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run waypoint controller
|
||||
failsafe_terrain_set_status(wp_nav->update_wpnav());
|
||||
_copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
|
||||
|
||||
// call z-axis position controller (wpnav should have already updated it's alt target)
|
||||
pos_control->update_z_controller();
|
||||
|
||||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
} else if (auto_yaw_mode == AUTO_YAW_RATE) {
|
||||
@ -461,7 +461,7 @@ void Copter::guided_pos_control_run()
|
||||
|
||||
// guided_vel_control_run - runs the guided velocity controller
|
||||
// called from guided_run
|
||||
void Copter::guided_vel_control_run()
|
||||
void Copter::FlightMode_GUIDED::vel_control_run()
|
||||
{
|
||||
// if not auto armed or motors not enabled set throttle to zero and exit immediately
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
|
||||
@ -481,7 +481,7 @@ void Copter::guided_vel_control_run()
|
||||
|
||||
// process pilot's yaw input
|
||||
float target_yaw_rate = 0;
|
||||
if (!failsafe.radio) {
|
||||
if (!_copter.failsafe.radio) {
|
||||
// get pilot's desired yaw rate
|
||||
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
||||
if (!is_zero(target_yaw_rate)) {
|
||||
@ -496,20 +496,20 @@ void Copter::guided_vel_control_run()
|
||||
uint32_t tnow = millis();
|
||||
if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
|
||||
if (!pos_control->get_desired_velocity().is_zero()) {
|
||||
guided_set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f, 0.0f, 0.0f));
|
||||
set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f, 0.0f, 0.0f));
|
||||
}
|
||||
if (auto_yaw_mode == AUTO_YAW_RATE) {
|
||||
set_auto_yaw_rate(0.0f);
|
||||
}
|
||||
} else {
|
||||
guided_set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms);
|
||||
set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms);
|
||||
}
|
||||
|
||||
// call velocity controller which includes z axis controller
|
||||
pos_control->update_vel_controller_xyz(ekfNavVelGainScaler);
|
||||
|
||||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
} else if (auto_yaw_mode == AUTO_YAW_RATE) {
|
||||
@ -523,7 +523,7 @@ void Copter::guided_vel_control_run()
|
||||
|
||||
// guided_posvel_control_run - runs the guided spline controller
|
||||
// called from guided_run
|
||||
void Copter::guided_posvel_control_run()
|
||||
void Copter::FlightMode_GUIDED::posvel_control_run()
|
||||
{
|
||||
// if not auto armed or motors not enabled set throttle to zero and exit immediately
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
|
||||
@ -545,7 +545,7 @@ void Copter::guided_posvel_control_run()
|
||||
// process pilot's yaw input
|
||||
float target_yaw_rate = 0;
|
||||
|
||||
if (!failsafe.radio) {
|
||||
if (!_copter.failsafe.radio) {
|
||||
// get pilot's desired yaw rate
|
||||
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
||||
if (!is_zero(target_yaw_rate)) {
|
||||
@ -589,7 +589,7 @@ void Copter::guided_posvel_control_run()
|
||||
pos_control->update_z_controller();
|
||||
|
||||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
} else if (auto_yaw_mode == AUTO_YAW_RATE) {
|
||||
@ -603,7 +603,7 @@ void Copter::guided_posvel_control_run()
|
||||
|
||||
// guided_angle_control_run - runs the guided angle controller
|
||||
// called from guided_run
|
||||
void Copter::guided_angle_control_run()
|
||||
void Copter::FlightMode_GUIDED::angle_control_run()
|
||||
{
|
||||
// if not auto armed or motors not enabled set throttle to zero and exit immediately
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) {
|
||||
@ -625,7 +625,7 @@ void Copter::guided_angle_control_run()
|
||||
float roll_in = guided_angle_state.roll_cd;
|
||||
float pitch_in = guided_angle_state.pitch_cd;
|
||||
float total_in = norm(roll_in, pitch_in);
|
||||
float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), aparm.angle_max);
|
||||
float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), _copter.aparm.angle_max);
|
||||
if (total_in > angle_max) {
|
||||
float ratio = angle_max / total_in;
|
||||
roll_in *= ratio;
|
||||
@ -667,7 +667,7 @@ void Copter::guided_angle_control_run()
|
||||
}
|
||||
|
||||
// helper function to update position controller's desired velocity while respecting acceleration limits
|
||||
void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des)
|
||||
void Copter::FlightMode_GUIDED::set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des)
|
||||
{
|
||||
// get current desired velocity
|
||||
Vector3f curr_vel_des = pos_control->get_desired_velocity();
|
||||
@ -691,7 +691,7 @@ void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vecto
|
||||
|
||||
#if AC_AVOID_ENABLED
|
||||
// limit the velocity to prevent fence violations
|
||||
avoid.adjust_velocity(pos_control->get_pos_xy_kP(), pos_control->get_accel_xy(), curr_vel_des);
|
||||
_copter.avoid.adjust_velocity(pos_control->get_pos_xy_kP(), pos_control->get_accel_xy(), curr_vel_des);
|
||||
// get avoidance adjusted climb rate
|
||||
curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z);
|
||||
#endif
|
||||
@ -701,7 +701,7 @@ void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vecto
|
||||
}
|
||||
|
||||
// helper function to set yaw state and targets
|
||||
void Copter::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
|
||||
void Copter::FlightMode_GUIDED::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
|
||||
{
|
||||
if (use_yaw) {
|
||||
set_auto_yaw_look_at_heading(yaw_cd / 100.0f, 0.0f, 0, relative_angle);
|
||||
@ -713,7 +713,7 @@ void Copter::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate,
|
||||
// Guided Limit code
|
||||
|
||||
// guided_limit_clear - clear/turn off guided limits
|
||||
void Copter::guided_limit_clear()
|
||||
void Copter::FlightMode_GUIDED::limit_clear()
|
||||
{
|
||||
guided_limit.timeout_ms = 0;
|
||||
guided_limit.alt_min_cm = 0.0f;
|
||||
@ -722,7 +722,7 @@ void Copter::guided_limit_clear()
|
||||
}
|
||||
|
||||
// guided_limit_set - set guided timeout and movement limits
|
||||
void Copter::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
|
||||
void Copter::FlightMode_GUIDED::limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
|
||||
{
|
||||
guided_limit.timeout_ms = timeout_ms;
|
||||
guided_limit.alt_min_cm = alt_min_cm;
|
||||
@ -732,7 +732,7 @@ void Copter::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_m
|
||||
|
||||
// guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking
|
||||
// only called from AUTO mode's auto_nav_guided_start function
|
||||
void Copter::guided_limit_init_time_and_pos()
|
||||
void Copter::FlightMode_GUIDED::limit_init_time_and_pos()
|
||||
{
|
||||
// initialise start time
|
||||
guided_limit.start_time = AP_HAL::millis();
|
||||
@ -743,7 +743,7 @@ void Copter::guided_limit_init_time_and_pos()
|
||||
|
||||
// guided_limit_check - returns true if guided mode has breached a limit
|
||||
// used when guided is invoked from the NAV_GUIDED_ENABLE mission command
|
||||
bool Copter::guided_limit_check()
|
||||
bool Copter::FlightMode_GUIDED::limit_check()
|
||||
{
|
||||
// check if we have passed the timeout
|
||||
if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) {
|
||||
|
||||
@ -8,7 +8,7 @@
|
||||
bool Copter::guided_nogps_init(bool ignore_checks)
|
||||
{
|
||||
// start in angle control mode
|
||||
guided_angle_control_start();
|
||||
flightmode_guided.angle_control_start();
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -17,6 +17,6 @@ bool Copter::guided_nogps_init(bool ignore_checks)
|
||||
void Copter::guided_nogps_run()
|
||||
{
|
||||
// run angle controller
|
||||
guided_angle_control_run();
|
||||
flightmode_guided.angle_control_run();
|
||||
}
|
||||
|
||||
|
||||
@ -82,7 +82,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
|
||||
break;
|
||||
|
||||
case GUIDED:
|
||||
success = guided_init(ignore_checks);
|
||||
success = flightmode_guided.init(ignore_checks);
|
||||
if (success) {
|
||||
flightmode = &flightmode_guided;
|
||||
}
|
||||
break;
|
||||
|
||||
case LAND:
|
||||
@ -203,10 +206,6 @@ void Copter::update_flight_mode()
|
||||
|
||||
switch (control_mode) {
|
||||
|
||||
case GUIDED:
|
||||
guided_run();
|
||||
break;
|
||||
|
||||
case LAND:
|
||||
land_run();
|
||||
break;
|
||||
@ -325,7 +324,6 @@ bool Copter::mode_requires_GPS()
|
||||
return flightmode->requires_GPS();
|
||||
}
|
||||
switch (control_mode) {
|
||||
case GUIDED:
|
||||
case RTL:
|
||||
case SMART_RTL:
|
||||
case DRIFT:
|
||||
@ -376,7 +374,6 @@ void Copter::notify_flight_mode()
|
||||
return;
|
||||
}
|
||||
switch (control_mode) {
|
||||
case GUIDED:
|
||||
case RTL:
|
||||
case AVOID_ADSB:
|
||||
case GUIDED_NOGPS:
|
||||
@ -393,9 +390,6 @@ void Copter::notify_flight_mode()
|
||||
|
||||
// set flight mode string
|
||||
switch (control_mode) {
|
||||
case GUIDED:
|
||||
notify.set_flight_mode_str("GUID");
|
||||
break;
|
||||
case RTL:
|
||||
notify.set_flight_mode_str("RTL ");
|
||||
break;
|
||||
|
||||
@ -37,11 +37,11 @@ void Copter::calc_wp_distance()
|
||||
break;
|
||||
|
||||
case GUIDED:
|
||||
if (guided_mode == Guided_WP) {
|
||||
if (flightmode_guided.mode() == Guided_WP) {
|
||||
wp_distance = wp_nav->get_wp_distance_to_destination();
|
||||
break;
|
||||
}
|
||||
if (guided_mode == Guided_PosVel) {
|
||||
if (flightmode_guided.mode() == Guided_PosVel) {
|
||||
wp_distance = pos_control->get_distance_to_target();
|
||||
break;
|
||||
}
|
||||
@ -69,11 +69,11 @@ void Copter::calc_wp_bearing()
|
||||
break;
|
||||
|
||||
case GUIDED:
|
||||
if (guided_mode == Guided_WP) {
|
||||
if (flightmode_guided.mode() == Guided_WP) {
|
||||
wp_bearing = wp_nav->get_wp_bearing_to_destination();
|
||||
break;
|
||||
}
|
||||
if (guided_mode == Guided_PosVel) {
|
||||
if (flightmode_guided.mode() == Guided_PosVel) {
|
||||
wp_bearing = pos_control->get_bearing_to_target();
|
||||
break;
|
||||
}
|
||||
|
||||
@ -36,7 +36,7 @@ bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
|
||||
|
||||
switch(control_mode) {
|
||||
case GUIDED:
|
||||
if (guided_takeoff_start(takeoff_alt_cm)) {
|
||||
if (flightmode_guided.takeoff_start(takeoff_alt_cm)) {
|
||||
set_auto_armed(true);
|
||||
return true;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user