mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-11 17:13:56 -03:00
Sub: Match Copter changes
This commit is contained in:
Rustom Jehangir
Andrew Tridgell
parent
37118920ed
commit
617b439d0e
@ -68,7 +68,7 @@
|
||||
* ..and many more.
|
||||
*
|
||||
* Code commit statistics can be found here: https://github.com/diydrones/ardupilot/graphs/contributors
|
||||
* Wiki: http://copter.ardupilot.com/
|
||||
* Wiki: http://copter.ardupilot.org/
|
||||
* Requires modified version of Arduino, which can be found here: http://ardupilot.com/downloads/?category=6
|
||||
*
|
||||
*/
|
||||
@ -129,6 +129,7 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
|
||||
#if FRSKY_TELEM_ENABLED == ENABLED
|
||||
SCHED_TASK(frsky_telemetry_send, 5, 75),
|
||||
#endif
|
||||
SCHED_TASK(terrain_update, 10, 100),
|
||||
#if EPM_ENABLED == ENABLED
|
||||
SCHED_TASK(epm_update, 10, 75),
|
||||
#endif
|
||||
@ -303,9 +304,6 @@ void Sub::rc_loop()
|
||||
// ---------------------------
|
||||
void Sub::throttle_loop()
|
||||
{
|
||||
// get altitude and climb rate from inertial lib
|
||||
read_inertial_altitude();
|
||||
|
||||
// update throttle_low_comp value (controls priority of throttle vs attitude control)
|
||||
update_throttle_thr_mix();
|
||||
|
||||
@ -457,6 +455,9 @@ void Sub::three_hz_loop()
|
||||
// check if we've lost contact with the ground station
|
||||
failsafe_gcs_check();
|
||||
|
||||
// check if we've lost terrain data
|
||||
failsafe_terrain_check();
|
||||
|
||||
#if AC_FENCE == ENABLED
|
||||
// check if we have breached a fence
|
||||
fence_check();
|
||||
@ -503,24 +504,14 @@ void Sub::one_hz_loop()
|
||||
|
||||
check_usb_mux();
|
||||
|
||||
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
|
||||
terrain.update();
|
||||
|
||||
// tell the rangefinder our height, so it can go into power saving
|
||||
// mode if available
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
float height;
|
||||
if (terrain.height_above_terrain(height, true)) {
|
||||
sonar.set_estimated_terrain_height(height);
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// update position controller alt limits
|
||||
update_poscon_alt_max();
|
||||
|
||||
// enable/disable raw gyro/accel logging
|
||||
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
|
||||
|
||||
// log terrain data
|
||||
terrain_logging();
|
||||
}
|
||||
|
||||
// called at 50hz
|
||||
|
||||
@ -33,7 +33,7 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan)
|
||||
uint32_t custom_mode = control_mode;
|
||||
|
||||
// set system as critical if any failsafe have triggered
|
||||
if (failsafe.radio || failsafe.battery || failsafe.gcs || failsafe.ekf) {
|
||||
if (failsafe.radio || failsafe.battery || failsafe.gcs || failsafe.ekf || failsafe.terrain) {
|
||||
system_status = MAV_STATE_CRITICAL;
|
||||
}
|
||||
|
||||
@ -715,6 +715,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
|
||||
|
||||
case MSG_FENCE_STATUS:
|
||||
case MSG_WIND:
|
||||
case MSG_POSITION_TARGET_GLOBAL_INT:
|
||||
// unused
|
||||
break;
|
||||
|
||||
@ -973,9 +974,9 @@ GCS_MAVLINK::data_stream_send(void)
|
||||
}
|
||||
|
||||
|
||||
void GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd)
|
||||
bool GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd)
|
||||
{
|
||||
sub.do_guided(cmd);
|
||||
return sub.do_guided(cmd);
|
||||
}
|
||||
|
||||
void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
|
||||
@ -1064,8 +1065,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_MISSION_ITEM_INT:
|
||||
{
|
||||
if (handle_mission_item(msg, sub.mission)) {
|
||||
sub.DataFlash.Log_Write_EntireMission(sub.mission);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
// read an individual command from EEPROM and send it to the GCS
|
||||
case MAVLINK_MSG_ID_MISSION_REQUEST: // MAV ID: 40
|
||||
case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
|
||||
case MAVLINK_MSG_ID_MISSION_REQUEST: // MAV ID: 40, 51
|
||||
{
|
||||
handle_mission_request(sub.mission, msg);
|
||||
break;
|
||||
|
||||
@ -306,6 +306,7 @@ struct PACKED log_Control_Tuning {
|
||||
int32_t baro_alt;
|
||||
int16_t desired_sonar_alt;
|
||||
int16_t sonar_alt;
|
||||
float terr_alt;
|
||||
int16_t desired_climb_rate;
|
||||
int16_t climb_rate;
|
||||
};
|
||||
@ -313,6 +314,12 @@ struct PACKED log_Control_Tuning {
|
||||
// Write a control tuning packet
|
||||
void Sub::Log_Write_Control_Tuning()
|
||||
{
|
||||
// get terrain altitude
|
||||
float terr_alt = 0.0f;
|
||||
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
|
||||
terrain.height_above_terrain(terr_alt, true);
|
||||
#endif
|
||||
|
||||
struct log_Control_Tuning pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
|
||||
time_us : AP_HAL::micros64(),
|
||||
@ -324,6 +331,7 @@ void Sub::Log_Write_Control_Tuning()
|
||||
baro_alt : baro_alt,
|
||||
desired_sonar_alt : (int16_t)target_sonar_alt,
|
||||
sonar_alt : sonar_alt,
|
||||
terr_alt : terr_alt,
|
||||
desired_climb_rate : (int16_t)pos_control.get_vel_target_z(),
|
||||
climb_rate : climb_rate
|
||||
};
|
||||
@ -655,6 +663,36 @@ void Sub::Log_Write_Precland()
|
||||
#endif // PRECISION_LANDING == ENABLED
|
||||
}
|
||||
|
||||
// precision landing logging
|
||||
struct PACKED log_GuidedTarget {
|
||||
LOG_PACKET_HEADER;
|
||||
uint64_t time_us;
|
||||
uint8_t type;
|
||||
float pos_target_x;
|
||||
float pos_target_y;
|
||||
float pos_target_z;
|
||||
float vel_target_x;
|
||||
float vel_target_y;
|
||||
float vel_target_z;
|
||||
};
|
||||
|
||||
// Write a Guided mode target
|
||||
void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target)
|
||||
{
|
||||
struct log_GuidedTarget pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG),
|
||||
time_us : AP_HAL::micros64(),
|
||||
type : target_type,
|
||||
pos_target_x : pos_target.x,
|
||||
pos_target_y : pos_target.y,
|
||||
pos_target_z : pos_target.z,
|
||||
vel_target_x : vel_target.x,
|
||||
vel_target_y : vel_target.y,
|
||||
vel_target_z : vel_target.z
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
const struct LogStructure Sub::log_structure[] = {
|
||||
LOG_COMMON_STRUCTURES,
|
||||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
@ -670,7 +708,7 @@ const struct LogStructure Sub::log_structure[] = {
|
||||
{ LOG_NAV_TUNING_MSG, sizeof(log_Nav_Tuning),
|
||||
"NTUN", "Qffffffffff", "TimeUS,DPosX,DPosY,PosX,PosY,DVelX,DVelY,VelX,VelY,DAccX,DAccY" },
|
||||
{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
|
||||
"CTUN", "Qfffffecchh", "TimeUS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" },
|
||||
"CTUN", "Qfffffeccfhh", "TimeUS,ThrIn,ABst,ThrOut,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt" },
|
||||
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
|
||||
"PM", "QHHIhBHI", "TimeUS,NLon,NLoop,MaxT,PMT,I2CErr,INSErr,LogDrop" },
|
||||
{ LOG_MOTBATT_MSG, sizeof(log_MotBatt),
|
||||
@ -693,6 +731,8 @@ const struct LogStructure Sub::log_structure[] = {
|
||||
"HELI", "Qff", "TimeUS,DRRPM,ERRPM" },
|
||||
{ LOG_PRECLAND_MSG, sizeof(log_Precland),
|
||||
"PL", "QBffffff", "TimeUS,Heal,bX,bY,eX,eY,pX,pY" },
|
||||
{ LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
|
||||
"GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" },
|
||||
};
|
||||
|
||||
#if CLI_ENABLED == ENABLED
|
||||
@ -785,6 +825,7 @@ void Sub::Log_Write_Baro(void) {}
|
||||
void Sub::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) {}
|
||||
void Sub::Log_Write_Home_And_Origin() {}
|
||||
void Sub::Log_Sensor_Health() {}
|
||||
void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {};
|
||||
|
||||
#if OPTFLOW == ENABLED
|
||||
void Sub::Log_Write_Optflow() {}
|
||||
|
||||
@ -138,6 +138,15 @@ const AP_Param::Info Sub::var_info[] = {
|
||||
// @User: Standard
|
||||
GSCALAR(rtl_altitude, "RTL_ALT", RTL_ALT),
|
||||
|
||||
// @Param: RTL_CONE_SLOPE
|
||||
// @DisplayName: RTL cone slope
|
||||
// @Description: Defines a cone above home which determines maximum climb
|
||||
// @Range: 0.5 10.0
|
||||
// @Increment: .1
|
||||
// @Values: 0:Disabled,1:Shallow,3:Steep
|
||||
// @User: Standard
|
||||
GSCALAR(rtl_cone_slope, "RTL_CONE_SLOPE", RTL_CONE_SLOPE),
|
||||
|
||||
// @Param: RTL_SPEED
|
||||
// @DisplayName: RTL speed
|
||||
// @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally while flying home. If this is set to zero, WPNAV_SPEED will be used instead.
|
||||
@ -1023,6 +1032,13 @@ const AP_Param::Info Sub::var_info[] = {
|
||||
// @User: Standard
|
||||
GSCALAR(throw_motor_start, "THROW_MOT_START", 0),
|
||||
|
||||
// @Param: TERRAIN_FOLLOW
|
||||
// @DisplayName: Terrain Following use control
|
||||
// @Description: This enables terrain following for RTL and LAND flight modes. To use this option TERRAIN_ENABLE must be 1 and the GCS must support sending terrain data to the aircraft. In RTL the RTL_ALT will be considered a height above the terrain. In LAND mode the vehicle will slow to LAND_SPEED 10m above terrain (instead of 10m above home). This parameter does not affect AUTO and Guided which use a per-command flag to determine if the height is above-home, absolute or above-terrain.
|
||||
// @Values: 0:Do Not Use in RTL and Land 1:Use in RTL and Land
|
||||
// @User: Standard
|
||||
GSCALAR(terrain_follow, "TERRAIN_FOLLOW", 0),
|
||||
|
||||
AP_VAREND
|
||||
};
|
||||
|
||||
|
||||
@ -180,6 +180,7 @@ public:
|
||||
k_param_disarm_delay,
|
||||
k_param_fs_crash_check,
|
||||
k_param_throw_motor_start,
|
||||
k_param_terrain_follow, // 94
|
||||
|
||||
// 97: RSSI
|
||||
k_param_rssi = 97,
|
||||
@ -217,7 +218,8 @@ public:
|
||||
// 135 : reserved for Solo until features merged with master
|
||||
//
|
||||
k_param_rtl_speed_cms = 135,
|
||||
k_param_fs_batt_curr_rtl, // 136
|
||||
k_param_fs_batt_curr_rtl,
|
||||
k_param_rtl_cone_slope, // 137
|
||||
|
||||
//
|
||||
// 140: Sensor parameters
|
||||
@ -403,6 +405,7 @@ public:
|
||||
|
||||
AP_Int16 rtl_altitude;
|
||||
AP_Int16 rtl_speed_cms;
|
||||
AP_Float rtl_cone_slope;
|
||||
AP_Float sonar_gain;
|
||||
|
||||
AP_Int8 failsafe_battery_enabled; // battery failsafe enabled
|
||||
@ -473,6 +476,7 @@ public:
|
||||
AP_Int16 gcs_pid_mask;
|
||||
|
||||
AP_Int8 throw_motor_start;
|
||||
AP_Int8 terrain_follow;
|
||||
|
||||
// RC channels
|
||||
RC_Channel rc_1;
|
||||
|
||||
@ -39,7 +39,6 @@ Sub::Sub(void) :
|
||||
guided_mode(Guided_TakeOff),
|
||||
rtl_state(RTL_InitialClimb),
|
||||
rtl_state_complete(false),
|
||||
rtl_alt(0.0f),
|
||||
circle_pilot_yaw_override(false),
|
||||
simple_cos_yaw(1.0f),
|
||||
simple_sin_yaw(0.0f),
|
||||
|
||||
@ -35,6 +35,7 @@
|
||||
|
||||
// Common dependencies
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_Common/Location.h>
|
||||
#include <AP_Menu/AP_Menu.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <StorageManager/StorageManager.h>
|
||||
@ -284,10 +285,13 @@ private:
|
||||
uint8_t battery : 1; // 2 // A status flag for the battery failsafe
|
||||
uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe
|
||||
uint8_t ekf : 1; // 5 // true if ekf failsafe has occurred
|
||||
uint8_t terrain : 1; // 6 // true if the missing terrain data failsafe has occurred
|
||||
|
||||
int8_t radio_counter; // number of iterations with throttle below throttle_fs_value
|
||||
|
||||
uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe
|
||||
uint32_t terrain_first_failure_ms; // the first time terrain data access failed - used to calculate the duration of the failure
|
||||
uint32_t terrain_last_failure_ms; // the most recent time terrain data access failed
|
||||
} failsafe;
|
||||
|
||||
// sensor health for logging
|
||||
@ -351,7 +355,16 @@ private:
|
||||
// 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
|
||||
float rtl_alt; // altitude the vehicle is returning at
|
||||
|
||||
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;
|
||||
bool land;
|
||||
bool terrain_used;
|
||||
} rtl_path;
|
||||
|
||||
// Circle
|
||||
bool circle_pilot_yaw_override; // true if pilot is overriding yaw
|
||||
@ -409,7 +422,7 @@ private:
|
||||
|
||||
// 3D Location vectors
|
||||
// Current location of the Sub (altitude is relative to home)
|
||||
struct Location current_loc;
|
||||
Location_Class current_loc;
|
||||
|
||||
// Navigation Yaw control
|
||||
// auto flight mode's yaw mode
|
||||
@ -645,6 +658,7 @@ private:
|
||||
void Log_Write_Home_And_Origin();
|
||||
void Log_Sensor_Health();
|
||||
void Log_Write_Precland();
|
||||
void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
|
||||
void Log_Write_Vehicle_Startup_Messages();
|
||||
void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
|
||||
void start_logging() ;
|
||||
@ -672,7 +686,6 @@ private:
|
||||
bool verify_loiter_time();
|
||||
bool verify_RTL();
|
||||
bool verify_wait_delay();
|
||||
bool verify_change_alt();
|
||||
bool verify_within_distance();
|
||||
bool verify_yaw();
|
||||
void do_take_picture();
|
||||
@ -686,9 +699,10 @@ private:
|
||||
void althold_run();
|
||||
bool auto_init(bool ignore_checks);
|
||||
void auto_run();
|
||||
void auto_takeoff_start(float final_alt_above_home);
|
||||
void auto_takeoff_start(const Location& dest_loc);
|
||||
void auto_takeoff_run();
|
||||
void auto_wp_start(const Vector3f& destination);
|
||||
void auto_wp_start(const Location_Class& dest_loc);
|
||||
void auto_wp_run();
|
||||
void auto_spline_run();
|
||||
void auto_land_start();
|
||||
@ -696,7 +710,7 @@ private:
|
||||
void auto_land_run();
|
||||
void auto_rtl_start();
|
||||
void auto_rtl_run();
|
||||
void auto_circle_movetoedge_start();
|
||||
void auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m);
|
||||
void auto_circle_start();
|
||||
void auto_circle_run();
|
||||
void auto_nav_guided_start();
|
||||
@ -740,12 +754,13 @@ private:
|
||||
bool flip_init(bool ignore_checks);
|
||||
void flip_run();
|
||||
bool guided_init(bool ignore_checks);
|
||||
void guided_takeoff_start(float final_alt_above_home);
|
||||
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();
|
||||
void guided_set_destination(const Vector3f& destination);
|
||||
bool guided_set_destination(const Location_Class& dest_loc);
|
||||
void guided_set_velocity(const Vector3f& velocity);
|
||||
void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity);
|
||||
void guided_set_angle(const Quaternion &q, float climb_rate_cms);
|
||||
@ -788,6 +803,7 @@ private:
|
||||
bool throw_height_good();
|
||||
|
||||
bool rtl_init(bool ignore_checks);
|
||||
void rtl_restart_without_terrain();
|
||||
void rtl_run();
|
||||
void rtl_climb_start();
|
||||
void rtl_return_start();
|
||||
@ -798,6 +814,8 @@ private:
|
||||
void rtl_descent_run();
|
||||
void rtl_land_start();
|
||||
void rtl_land_run();
|
||||
void rtl_build_path(bool terrain_following_allowed);
|
||||
void rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target, bool terrain_following_allowed);
|
||||
float get_RTL_alt();
|
||||
bool sport_init(bool ignore_checks);
|
||||
void sport_run();
|
||||
@ -822,6 +840,9 @@ private:
|
||||
void failsafe_battery_event(void);
|
||||
void failsafe_gcs_check();
|
||||
void failsafe_gcs_off_event(void);
|
||||
void failsafe_terrain_check();
|
||||
void failsafe_terrain_set_status(bool data_ok);
|
||||
void failsafe_terrain_on_event();
|
||||
void set_mode_RTL_or_land_with_pause(mode_reason_t reason);
|
||||
void update_events();
|
||||
void failsafe_enable();
|
||||
@ -864,6 +885,7 @@ private:
|
||||
void pre_arm_rc_checks();
|
||||
bool pre_arm_gps_checks(bool display_failure);
|
||||
bool pre_arm_ekf_attitude_check();
|
||||
bool pre_arm_terrain_check();
|
||||
bool arm_checks(bool display_failure, bool arming_from_gcs);
|
||||
void init_disarm_motors();
|
||||
void motors_output();
|
||||
@ -912,6 +934,9 @@ private:
|
||||
void read_battery(void);
|
||||
void read_receiver_rssi(void);
|
||||
void epm_update();
|
||||
void terrain_update();
|
||||
void terrain_logging();
|
||||
bool terrain_use();
|
||||
void report_batt_monitor();
|
||||
void report_frame();
|
||||
void report_radio();
|
||||
@ -974,7 +999,6 @@ private:
|
||||
#endif
|
||||
void do_wait_delay(const AP_Mission::Mission_Command& cmd);
|
||||
void do_within_distance(const AP_Mission::Mission_Command& cmd);
|
||||
void do_change_alt(const AP_Mission::Mission_Command& cmd);
|
||||
void do_yaw(const AP_Mission::Mission_Command& cmd);
|
||||
void do_change_speed(const AP_Mission::Mission_Command& cmd);
|
||||
void do_set_home(const AP_Mission::Mission_Command& cmd);
|
||||
@ -996,8 +1020,7 @@ private:
|
||||
#if NAV_GUIDED == ENABLED
|
||||
bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
|
||||
#endif
|
||||
void auto_spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_spline_destination);
|
||||
|
||||
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 print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
|
||||
void log_init(void);
|
||||
void run_cli(AP_HAL::UARTDriver *port);
|
||||
|
||||
@ -318,6 +318,12 @@ bool Sub::pre_arm_checks(bool display_failure)
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
// check for missing terrain data
|
||||
if (!pre_arm_terrain_check()) {
|
||||
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Terrain data");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// check throttle is above failsafe throttle
|
||||
@ -468,6 +474,24 @@ bool Sub::pre_arm_ekf_attitude_check()
|
||||
return filt_status.flags.attitude;
|
||||
}
|
||||
|
||||
// check we have required terrain data
|
||||
bool Sub::pre_arm_terrain_check()
|
||||
{
|
||||
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
|
||||
// succeed if not using terrain data
|
||||
if (!terrain_use()) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// show terrain statistics
|
||||
uint16_t terr_pending, terr_loaded;
|
||||
terrain.get_statistics(terr_pending, terr_loaded);
|
||||
return (terr_pending <= 0);
|
||||
#else
|
||||
return true;
|
||||
#endif
|
||||
}
|
||||
|
||||
// arm_checks - perform final checks before arming
|
||||
// always called just before arming. Return true if ok to arm
|
||||
// has side-effect that logging is started
|
||||
@ -611,6 +635,14 @@ bool Sub::arm_checks(bool display_failure, bool arming_from_gcs)
|
||||
}
|
||||
}
|
||||
|
||||
// check for missing terrain data
|
||||
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) {
|
||||
if (!pre_arm_terrain_check()) {
|
||||
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Waiting for Terrain data");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// check throttle
|
||||
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) {
|
||||
// check throttle is not too low - must be above failsafe throttle
|
||||
|
||||
@ -6,8 +6,10 @@ void Sub::init_capabilities(void)
|
||||
{
|
||||
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT);
|
||||
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT);
|
||||
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_INT);
|
||||
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED);
|
||||
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT);
|
||||
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION);
|
||||
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET);
|
||||
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_TERRAIN);
|
||||
}
|
||||
|
||||
@ -64,10 +64,6 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
|
||||
do_within_distance(cmd);
|
||||
break;
|
||||
|
||||
case MAV_CMD_CONDITION_CHANGE_ALT: // 113
|
||||
do_change_alt(cmd);
|
||||
break;
|
||||
|
||||
case MAV_CMD_CONDITION_YAW: // 115
|
||||
do_yaw(cmd);
|
||||
break;
|
||||
@ -225,9 +221,6 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
|
||||
case MAV_CMD_CONDITION_DISTANCE:
|
||||
return verify_within_distance();
|
||||
|
||||
case MAV_CMD_CONDITION_CHANGE_ALT:
|
||||
return verify_change_alt();
|
||||
|
||||
case MAV_CMD_CONDITION_YAW:
|
||||
return verify_yaw();
|
||||
|
||||
@ -284,25 +277,20 @@ void Sub::do_RTL(void)
|
||||
void Sub::do_takeoff(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// Set wp navigation target to safe altitude above current position
|
||||
float takeoff_alt = cmd.content.location.alt;
|
||||
takeoff_alt = MAX(takeoff_alt,current_loc.alt);
|
||||
takeoff_alt = MAX(takeoff_alt,100.0f);
|
||||
auto_takeoff_start(takeoff_alt);
|
||||
auto_takeoff_start(cmd.content.location);
|
||||
}
|
||||
|
||||
// do_nav_wp - initiate move to next waypoint
|
||||
void Sub::do_nav_wp(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
const Vector3f &curr_pos = inertial_nav.get_position();
|
||||
const Vector3f local_pos = pv_location_to_vector_with_default(cmd.content.location, curr_pos);
|
||||
|
||||
// this will be used to remember the time in millis after we reach or pass the WP.
|
||||
loiter_time = 0;
|
||||
// this is the delay, stored in seconds
|
||||
loiter_time_max = abs(cmd.p1);
|
||||
loiter_time_max = cmd.p1;
|
||||
|
||||
// Set wp navigation target
|
||||
auto_wp_start(local_pos);
|
||||
auto_wp_start(Location_Class(cmd.content.location));
|
||||
|
||||
// if no delay set the waypoint as "fast"
|
||||
if (loiter_time_max == 0 ) {
|
||||
wp_nav.set_fast_waypoint(true);
|
||||
@ -320,9 +308,21 @@ void Sub::do_land(const AP_Mission::Mission_Command& cmd)
|
||||
land_state = LAND_STATE_FLY_TO_LOCATION;
|
||||
|
||||
// calculate and set desired location above landing target
|
||||
Vector3f pos = pv_location_to_vector(cmd.content.location);
|
||||
pos.z = inertial_nav.get_altitude();
|
||||
auto_wp_start(pos);
|
||||
// convert to location class
|
||||
Location_Class target_loc(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_loc.get_alt_cm(Location_Class::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);
|
||||
} else {
|
||||
// set target altitude to current altitude above home
|
||||
target_loc.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME);
|
||||
}
|
||||
auto_wp_start(target_loc);
|
||||
}else{
|
||||
// set landing state
|
||||
land_state = LAND_STATE_DESCENDING;
|
||||
@ -336,95 +336,72 @@ void Sub::do_land(const AP_Mission::Mission_Command& cmd)
|
||||
// note: caller should set yaw_mode
|
||||
void Sub::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
Vector3f target_pos;
|
||||
|
||||
// get current position
|
||||
Vector3f curr_pos = inertial_nav.get_position();
|
||||
|
||||
// default to use position provided
|
||||
target_pos = pv_location_to_vector(cmd.content.location);
|
||||
// convert back to location
|
||||
Location_Class target_loc(cmd.content.location);
|
||||
|
||||
// use current location if not provided
|
||||
if(cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
|
||||
wp_nav.get_wp_stopping_point_xy(target_pos);
|
||||
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);
|
||||
target_loc.offset(temp_pos.x * 100.0f, temp_pos.y * 100.0f);
|
||||
}
|
||||
|
||||
// use current altitude if not provided
|
||||
// To-Do: use z-axis stopping point instead of current alt
|
||||
if( cmd.content.location.alt == 0 ) {
|
||||
target_pos.z = curr_pos.z;
|
||||
if (target_loc.alt == 0) {
|
||||
// set to current altitude but in command's alt frame
|
||||
int32_t curr_alt;
|
||||
if (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(current_loc.alt, current_loc.get_alt_frame());
|
||||
}
|
||||
}
|
||||
|
||||
// start way point navigator and provide it the desired location
|
||||
auto_wp_start(target_pos);
|
||||
auto_wp_start(target_loc);
|
||||
}
|
||||
|
||||
// do_circle - initiate moving in a circle
|
||||
void Sub::do_circle(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
Vector3f curr_pos = inertial_nav.get_position();
|
||||
Vector3f circle_center = pv_location_to_vector(cmd.content.location);
|
||||
Location_Class 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?
|
||||
if (circle_center.lat == 0 && circle_center.lng == 0) {
|
||||
circle_center.lat = current_loc.lat;
|
||||
circle_center.lng = current_loc.lng;
|
||||
}
|
||||
|
||||
// default target altitude to current altitude if not provided
|
||||
if (circle_center.alt == 0) {
|
||||
int32_t curr_alt;
|
||||
if (current_loc.get_alt_cm(circle_center.get_alt_frame(),curr_alt)) {
|
||||
// circle altitude uses frame from command
|
||||
circle_center.set_alt_cm(curr_alt,circle_center.get_alt_frame());
|
||||
} else {
|
||||
// default to current altitude above origin
|
||||
circle_center.set_alt_cm(current_loc.alt, current_loc.get_alt_frame());
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
|
||||
}
|
||||
}
|
||||
|
||||
// calculate radius
|
||||
uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
|
||||
bool move_to_edge_required = false;
|
||||
|
||||
// set target altitude if not provided
|
||||
if (cmd.content.location.alt == 0) {
|
||||
circle_center.z = curr_pos.z;
|
||||
} else {
|
||||
move_to_edge_required = true;
|
||||
}
|
||||
|
||||
// set lat/lon position if not provided
|
||||
// To-Do: use previous command's destination if it was a straight line or spline waypoint command
|
||||
if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
|
||||
circle_center.x = curr_pos.x;
|
||||
circle_center.y = curr_pos.y;
|
||||
} else {
|
||||
move_to_edge_required = true;
|
||||
}
|
||||
|
||||
// set circle controller's center
|
||||
circle_nav.set_center(circle_center);
|
||||
|
||||
// set circle radius
|
||||
if (circle_radius_m != 0) {
|
||||
circle_nav.set_radius((float)circle_radius_m * 100.0f);
|
||||
}
|
||||
|
||||
// check if we need to move to edge of circle
|
||||
if (move_to_edge_required) {
|
||||
// move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
|
||||
auto_circle_movetoedge_start();
|
||||
} else {
|
||||
// start circling
|
||||
auto_circle_start();
|
||||
}
|
||||
// move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
|
||||
auto_circle_movetoedge_start(circle_center, circle_radius_m);
|
||||
}
|
||||
|
||||
// do_loiter_time - initiate loitering at a point for a given time period
|
||||
// note: caller should set yaw_mode
|
||||
void Sub::do_loiter_time(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
Vector3f target_pos;
|
||||
|
||||
// get current position
|
||||
Vector3f curr_pos = inertial_nav.get_position();
|
||||
|
||||
// default to use position provided
|
||||
target_pos = pv_location_to_vector(cmd.content.location);
|
||||
|
||||
// use current location if not provided
|
||||
if(cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
|
||||
wp_nav.get_wp_stopping_point_xy(target_pos);
|
||||
}
|
||||
|
||||
// use current altitude if not provided
|
||||
if( cmd.content.location.alt == 0 ) {
|
||||
target_pos.z = curr_pos.z;
|
||||
}
|
||||
|
||||
// start way point navigator and provide it the desired location
|
||||
auto_wp_start(target_pos);
|
||||
// re-use loiter unlimited
|
||||
do_loiter_unlimited(cmd);
|
||||
|
||||
// setup loiter timer
|
||||
loiter_time = 0;
|
||||
@ -434,19 +411,33 @@ 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)
|
||||
{
|
||||
const Vector3f& curr_pos = inertial_nav.get_position();
|
||||
Vector3f local_pos = pv_location_to_vector_with_default(cmd.content.location, curr_pos);
|
||||
Location_Class 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;
|
||||
target_loc.lng = current_loc.lng;
|
||||
}
|
||||
// use current altitude if not provided
|
||||
if (target_loc.alt == 0) {
|
||||
// set to current altitude but in command's alt frame
|
||||
int32_t curr_alt;
|
||||
if (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(current_loc.alt, current_loc.get_alt_frame());
|
||||
}
|
||||
}
|
||||
|
||||
// this will be used to remember the time in millis after we reach or pass the WP.
|
||||
loiter_time = 0;
|
||||
// this is the delay, stored in seconds
|
||||
loiter_time_max = abs(cmd.p1);
|
||||
loiter_time_max = cmd.p1;
|
||||
|
||||
// determine segment start and end type
|
||||
bool stopped_at_start = true;
|
||||
AC_WPNav::spline_segment_end_type seg_end_type = AC_WPNav::SEGMENT_END_STOP;
|
||||
AP_Mission::Mission_Command temp_cmd;
|
||||
Vector3f next_destination; // end of next segment
|
||||
|
||||
// if previous command was a wp_nav command with no delay set stopped_at_start to false
|
||||
// To-Do: move processing of delay into wp-nav controller to allow it to determine the stopped_at_start value itself?
|
||||
@ -460,19 +451,34 @@ 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;
|
||||
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
|
||||
if (next_loc.lat == 0 && next_loc.lng == 0) {
|
||||
next_loc.lat = target_loc.lat;
|
||||
next_loc.lng = target_loc.lng;
|
||||
}
|
||||
// default alt to first waypoint's alt but in next waypoint's alt frame
|
||||
if (next_loc.alt == 0) {
|
||||
int32_t next_alt;
|
||||
if (target_loc.get_alt_cm(next_loc.get_alt_frame(), next_alt)) {
|
||||
next_loc.set_alt_cm(next_alt, next_loc.get_alt_frame());
|
||||
} else {
|
||||
// default to first waypoints altitude
|
||||
next_loc.set_alt_cm(target_loc.alt, target_loc.get_alt_frame());
|
||||
}
|
||||
}
|
||||
// if the next nav command is a waypoint set end type to spline or straight
|
||||
if (temp_cmd.id == MAV_CMD_NAV_WAYPOINT) {
|
||||
seg_end_type = AC_WPNav::SEGMENT_END_STRAIGHT;
|
||||
next_destination = pv_location_to_vector_with_default(temp_cmd.content.location, local_pos);
|
||||
}else if (temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) {
|
||||
seg_end_type = AC_WPNav::SEGMENT_END_SPLINE;
|
||||
next_destination = pv_location_to_vector_with_default(temp_cmd.content.location, local_pos);
|
||||
}
|
||||
}
|
||||
|
||||
// set spline navigation target
|
||||
auto_spline_start(local_pos, stopped_at_start, seg_end_type, next_destination);
|
||||
auto_spline_start(target_loc, stopped_at_start, seg_end_type, next_loc);
|
||||
}
|
||||
|
||||
#if NAV_GUIDED == ENABLED
|
||||
@ -724,11 +730,6 @@ void Sub::do_wait_delay(const AP_Mission::Mission_Command& cmd)
|
||||
condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds
|
||||
}
|
||||
|
||||
void Sub::do_change_alt(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// To-Do: store desired altitude in a variable so that it can be verified later
|
||||
}
|
||||
|
||||
void Sub::do_within_distance(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
condition_value = cmd.content.distance.meters * 100;
|
||||
@ -757,12 +758,6 @@ bool Sub::verify_wait_delay()
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Sub::verify_change_alt()
|
||||
{
|
||||
// To-Do: use recorded target altitude to verify we have reached the target
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Sub::verify_within_distance()
|
||||
{
|
||||
// update distance calculation
|
||||
@ -797,8 +792,6 @@ bool Sub::verify_yaw()
|
||||
// do_guided - start guided mode
|
||||
bool Sub::do_guided(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
Vector3f pos_or_vel; // target location or velocity
|
||||
|
||||
// only process guided waypoint if we are in guided mode
|
||||
if (control_mode != GUIDED && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) {
|
||||
return false;
|
||||
@ -808,11 +801,12 @@ bool Sub::do_guided(const AP_Mission::Mission_Command& cmd)
|
||||
switch (cmd.id) {
|
||||
|
||||
case MAV_CMD_NAV_WAYPOINT:
|
||||
{
|
||||
// set wp_nav's destination
|
||||
pos_or_vel = pv_location_to_vector(cmd.content.location);
|
||||
guided_set_destination(pos_or_vel);
|
||||
return true;
|
||||
Location_Class dest(cmd.content.location);
|
||||
return guided_set_destination(dest);
|
||||
break;
|
||||
}
|
||||
|
||||
case MAV_CMD_CONDITION_YAW:
|
||||
do_yaw(cmd);
|
||||
|
||||
@ -31,6 +31,11 @@ uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan)
|
||||
ap.compass_mot = true;
|
||||
}
|
||||
|
||||
// initialise output
|
||||
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
|
||||
interference_pct[i] = 0.0f;
|
||||
}
|
||||
|
||||
// check compass is enabled
|
||||
if (!g.compass_enabled) {
|
||||
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Compass disabled");
|
||||
|
||||
@ -171,6 +171,11 @@
|
||||
#define FS_RADIO_TIMEOUT_MS 500 // RC Radio Failsafe triggers after 500 miliseconds with No RC Input
|
||||
#endif
|
||||
|
||||
// missing terrain data failsafe
|
||||
#ifndef FS_TERRAIN_TIMEOUT_MS
|
||||
#define FS_TERRAIN_TIMEOUT_MS 5000 // 5 seconds of missing terrain data will trigger failsafe (RTL)
|
||||
#endif
|
||||
|
||||
#ifndef PREARM_DISPLAY_PERIOD
|
||||
# define PREARM_DISPLAY_PERIOD 30
|
||||
#endif
|
||||
@ -411,6 +416,18 @@
|
||||
# define RTL_CLIMB_MIN_DEFAULT 0 // vehicle will always climb this many cm as first stage of RTL
|
||||
#endif
|
||||
|
||||
#ifndef RTL_ABS_MIN_CLIMB
|
||||
# define RTL_ABS_MIN_CLIMB 250 // absolute minimum initial climb
|
||||
#endif
|
||||
|
||||
#ifndef RTL_CONE_SLOPE
|
||||
# define RTL_CONE_SLOPE 3.0f // slope of RTL cone (height / distance). 0 = No cone
|
||||
#endif
|
||||
|
||||
#ifndef RTL_MIN_CONE_SLOPE
|
||||
# define RTL_MIN_CONE_SLOPE 0.5f // minimum slope of cone
|
||||
#endif
|
||||
|
||||
#ifndef RTL_LOITER_TIME
|
||||
# define RTL_LOITER_TIME 5000 // Time (in milliseconds) to loiter above home before begining final descent
|
||||
#endif
|
||||
|
||||
@ -91,14 +91,41 @@ void Sub::auto_run()
|
||||
}
|
||||
|
||||
// auto_takeoff_start - initialises waypoint controller to implement take-off
|
||||
void Sub::auto_takeoff_start(float final_alt_above_home)
|
||||
void Sub::auto_takeoff_start(const Location& dest_loc)
|
||||
{
|
||||
auto_mode = Auto_TakeOff;
|
||||
|
||||
// initialise wpnav destination
|
||||
Vector3f target_pos = inertial_nav.get_position();
|
||||
target_pos.z = pv_alt_above_origin(final_alt_above_home);
|
||||
wp_nav.set_wp_destination(target_pos);
|
||||
// convert location to class
|
||||
Location_Class dest(dest_loc);
|
||||
|
||||
// set horizontal target
|
||||
dest.lat = current_loc.lat;
|
||||
dest.lng = current_loc.lng;
|
||||
|
||||
// get altitude target
|
||||
int32_t alt_target;
|
||||
if (!dest.get_alt_cm(Location_Class::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
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
|
||||
// fall back to altitude above current altitude
|
||||
alt_target = current_loc.alt + dest.alt;
|
||||
}
|
||||
|
||||
// sanity check target
|
||||
if (alt_target < current_loc.alt) {
|
||||
dest.set_alt_cm(current_loc.alt, Location_Class::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);
|
||||
}
|
||||
|
||||
// set waypoint controller target
|
||||
if (!wp_nav.set_wp_destination(dest)) {
|
||||
// failure to set destination can only be because of missing terrain data
|
||||
failsafe_terrain_on_event();
|
||||
return;
|
||||
}
|
||||
|
||||
// initialise yaw
|
||||
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
||||
@ -135,7 +162,7 @@ void Sub::auto_takeoff_run()
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run waypoint controller
|
||||
wp_nav.update_wpnav();
|
||||
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();
|
||||
@ -149,8 +176,26 @@ void Sub::auto_wp_start(const Vector3f& destination)
|
||||
{
|
||||
auto_mode = Auto_WP;
|
||||
|
||||
// initialise wpnav
|
||||
wp_nav.set_wp_destination(destination);
|
||||
// initialise wpnav (no need to check return status because terrain data is not used)
|
||||
wp_nav.set_wp_destination(destination, false);
|
||||
|
||||
// initialise yaw
|
||||
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
|
||||
if (auto_yaw_mode != AUTO_YAW_ROI) {
|
||||
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
|
||||
}
|
||||
}
|
||||
|
||||
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
|
||||
void Sub::auto_wp_start(const Location_Class& dest_loc)
|
||||
{
|
||||
auto_mode = Auto_WP;
|
||||
|
||||
// send target to waypoint controller
|
||||
if (!wp_nav.set_wp_destination(dest_loc)) {
|
||||
// failure to set destination can only be because of missing terrain data
|
||||
failsafe_terrain_on_event();
|
||||
}
|
||||
|
||||
// initialise yaw
|
||||
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
|
||||
@ -191,7 +236,7 @@ void Sub::auto_wp_run()
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run waypoint controller
|
||||
wp_nav.update_wpnav();
|
||||
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();
|
||||
@ -208,14 +253,18 @@ 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 Vector3f& destination, bool stopped_at_start,
|
||||
void Sub::auto_spline_start(const Location_Class& destination, bool stopped_at_start,
|
||||
AC_WPNav::spline_segment_end_type seg_end_type,
|
||||
const Vector3f& next_destination)
|
||||
const Location_Class& next_destination)
|
||||
{
|
||||
auto_mode = Auto_Spline;
|
||||
|
||||
// initialise wpnav
|
||||
wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination);
|
||||
if (!wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination)) {
|
||||
// failure to set destination (likely because of missing terrain data)
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
|
||||
// To-Do: handle failure
|
||||
}
|
||||
|
||||
// initialise yaw
|
||||
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
|
||||
@ -383,37 +432,65 @@ void Sub::auto_rtl_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()
|
||||
void Sub::auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m)
|
||||
{
|
||||
// check our distance from edge of circle
|
||||
Vector3f circle_edge;
|
||||
circle_nav.get_closest_point_on_circle(circle_edge);
|
||||
// convert location to vector from ekf origin
|
||||
Vector3f circle_center_neu;
|
||||
if (!circle_center.get_vector_from_origin_NEU(circle_center_neu)) {
|
||||
// default to current position and log error
|
||||
circle_center_neu = inertial_nav.get_position();
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_CIRCLE_INIT);
|
||||
}
|
||||
circle_nav.set_center(circle_center_neu);
|
||||
|
||||
// set the state to move to the edge of the circle
|
||||
auto_mode = Auto_CircleMoveToEdge;
|
||||
// set circle radius
|
||||
if (!is_zero(radius_m)) {
|
||||
circle_nav.set_radius(radius_m * 100.0f);
|
||||
}
|
||||
|
||||
// initialise wpnav to move to edge of circle
|
||||
wp_nav.set_wp_destination(circle_edge);
|
||||
// check our distance from edge of circle
|
||||
Vector3f circle_edge_neu;
|
||||
circle_nav.get_closest_point_on_circle(circle_edge_neu);
|
||||
float dist_to_edge = (inertial_nav.get_position() - circle_edge_neu).length();
|
||||
|
||||
// if we are outside the circle, point at the edge, otherwise hold yaw
|
||||
const Vector3f &curr_pos = inertial_nav.get_position();
|
||||
const Vector3f &circle_center = circle_nav.get_center();
|
||||
float dist_to_center = pythagorous2(circle_center.x - curr_pos.x, circle_center.y - curr_pos.y);
|
||||
if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {
|
||||
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
|
||||
} else {
|
||||
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
|
||||
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
||||
}
|
||||
// if more than 3m then fly to edge
|
||||
if (dist_to_edge > 300.0f) {
|
||||
// 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 altitude to same as command
|
||||
circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame());
|
||||
|
||||
// initialise wpnav to move to edge of circle
|
||||
if (!wp_nav.set_wp_destination(circle_edge)) {
|
||||
// failure to set destination can only be because of missing terrain data
|
||||
failsafe_terrain_on_event();
|
||||
}
|
||||
|
||||
// if we are outside the circle, point at the edge, otherwise hold yaw
|
||||
const Vector3f &curr_pos = inertial_nav.get_position();
|
||||
float dist_to_center = pythagorous2(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y);
|
||||
if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {
|
||||
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
|
||||
} else {
|
||||
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
|
||||
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
||||
}
|
||||
} else {
|
||||
auto_circle_start();
|
||||
}
|
||||
}
|
||||
|
||||
// auto_circle_start - initialises controller to fly a circle in AUTO flight mode
|
||||
// assumes that circle_nav object has already been initialised with circle center and radius
|
||||
void Sub::auto_circle_start()
|
||||
{
|
||||
auto_mode = Auto_Circle;
|
||||
|
||||
// initialise circle controller
|
||||
// center was set in do_circle so initialise with current center
|
||||
circle_nav.init(circle_nav.get_center());
|
||||
}
|
||||
|
||||
@ -501,8 +578,9 @@ void Sub::auto_loiter_run()
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run waypoint and z-axis postion controller
|
||||
wp_nav.update_wpnav();
|
||||
// run waypoint and z-axis position controller
|
||||
failsafe_terrain_set_status(wp_nav.update_wpnav());
|
||||
|
||||
pos_control.update_z_controller();
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
}
|
||||
|
||||
@ -51,41 +51,51 @@ bool Sub::guided_init(bool ignore_checks)
|
||||
|
||||
|
||||
// guided_takeoff_start - initialises waypoint controller to implement take-off
|
||||
void Sub::guided_takeoff_start(float final_alt_above_home)
|
||||
bool Sub::guided_takeoff_start(float final_alt_above_home)
|
||||
{
|
||||
guided_mode = Guided_TakeOff;
|
||||
|
||||
// initialise wpnav destination
|
||||
Vector3f target_pos = inertial_nav.get_position();
|
||||
target_pos.z = pv_alt_above_origin(final_alt_above_home);
|
||||
wp_nav.set_wp_destination(target_pos);
|
||||
guided_mode = Guided_TakeOff;
|
||||
|
||||
// initialise yaw
|
||||
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
||||
// initialise wpnav destination
|
||||
Location_Class target_loc = current_loc;
|
||||
target_loc.set_alt_cm(final_alt_above_home, Location_Class::ALT_FRAME_ABOVE_HOME);
|
||||
|
||||
// clear i term when we're taking off
|
||||
set_throttle_takeoff();
|
||||
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);
|
||||
// failure is propagated to GCS with NAK
|
||||
return false;
|
||||
}
|
||||
|
||||
// initialise yaw
|
||||
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
||||
|
||||
// clear i term when we're taking off
|
||||
set_throttle_takeoff();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// initialise guided mode's position controller
|
||||
void Sub::guided_pos_control_start()
|
||||
{
|
||||
// set to position control mode
|
||||
guided_mode = Guided_WP;
|
||||
// set to position control mode
|
||||
guided_mode = Guided_WP;
|
||||
|
||||
// initialise waypoint and spline controller
|
||||
wp_nav.wp_and_spline_init();
|
||||
// initialise waypoint and spline controller
|
||||
wp_nav.wp_and_spline_init();
|
||||
|
||||
// initialise wpnav to stopping point at current altitude
|
||||
// To-Do: set to current location if disarmed?
|
||||
// To-Do: set to stopping point altitude?
|
||||
Vector3f stopping_point;
|
||||
stopping_point.z = inertial_nav.get_altitude();
|
||||
wp_nav.get_wp_stopping_point_xy(stopping_point);
|
||||
wp_nav.set_wp_destination(stopping_point);
|
||||
// initialise wpnav to stopping point at current altitude
|
||||
// To-Do: set to current location if disarmed?
|
||||
// To-Do: set to stopping point altitude?
|
||||
Vector3f stopping_point;
|
||||
stopping_point.z = inertial_nav.get_altitude();
|
||||
wp_nav.get_wp_stopping_point_xy(stopping_point);
|
||||
|
||||
// initialise yaw
|
||||
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
|
||||
// no need to check return status because terrain data is not used
|
||||
wp_nav.set_wp_destination(stopping_point, false);
|
||||
|
||||
// initialise yaw
|
||||
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
|
||||
}
|
||||
|
||||
// initialise guided mode's velocity controller
|
||||
@ -163,7 +173,32 @@ void Sub::guided_set_destination(const Vector3f& destination)
|
||||
guided_pos_control_start();
|
||||
}
|
||||
|
||||
wp_nav.set_wp_destination(destination);
|
||||
// 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());
|
||||
}
|
||||
|
||||
// sets guided mode's target from a Location object
|
||||
// returns false if destination could not be set (probably caused by missing terrain data)
|
||||
bool Sub::guided_set_destination(const Location_Class& dest_loc)
|
||||
{
|
||||
// ensure we are in position control mode
|
||||
if (guided_mode != Guided_WP) {
|
||||
guided_pos_control_start();
|
||||
}
|
||||
|
||||
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);
|
||||
// failure is propagated to GCS with NAK
|
||||
return false;
|
||||
}
|
||||
|
||||
// log target
|
||||
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
|
||||
@ -270,7 +305,7 @@ void Sub::guided_takeoff_run()
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run waypoint controller
|
||||
wp_nav.update_wpnav();
|
||||
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();
|
||||
@ -306,7 +341,7 @@ void Sub::guided_pos_control_run()
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run waypoint controller
|
||||
wp_nav.update_wpnav();
|
||||
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();
|
||||
|
||||
@ -221,8 +221,21 @@ float Sub::get_land_descent_speed()
|
||||
#else
|
||||
bool sonar_ok = false;
|
||||
#endif
|
||||
|
||||
// get position controller's target altitude above terrain
|
||||
Location_Class target_loc = pos_control.get_pos_target();
|
||||
int32_t target_alt_cm;
|
||||
|
||||
// get altitude target above home by default
|
||||
target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, target_alt_cm);
|
||||
|
||||
// try to use terrain if enabled
|
||||
if (terrain_use() && !target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_alt_cm)) {
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
|
||||
}
|
||||
|
||||
// if we are above 10m and the sonar does not sense anything perform regular alt hold descent
|
||||
if (pos_control.get_pos_target().z <= pv_alt_above_origin(LAND_START_ALT) && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
|
||||
if (target_alt_cm <= LAND_START_ALT && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
|
||||
if (g.land_speed_high > 0) {
|
||||
// user has asked for a different landing speed than normal descent rate
|
||||
return -abs(g.land_speed_high);
|
||||
|
||||
@ -13,6 +13,7 @@
|
||||
bool Sub::rtl_init(bool ignore_checks)
|
||||
{
|
||||
if (position_ok() || ignore_checks) {
|
||||
rtl_build_path(true);
|
||||
rtl_climb_start();
|
||||
return true;
|
||||
}else{
|
||||
@ -20,6 +21,18 @@ bool Sub::rtl_init(bool ignore_checks)
|
||||
}
|
||||
}
|
||||
|
||||
// re-start RTL with terrain following disabled
|
||||
void Sub::rtl_restart_without_terrain()
|
||||
{
|
||||
// log an error
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_RESTARTED_RTL);
|
||||
if (rtl_path.terrain_used) {
|
||||
rtl_build_path(false);
|
||||
rtl_climb_start();
|
||||
gcs_send_text(MAV_SEVERITY_CRITICAL,"Restarting RTL - Terrain data missing");
|
||||
}
|
||||
}
|
||||
|
||||
// rtl_run - runs the return-to-launch controller
|
||||
// should be called at 100hz or more
|
||||
void Sub::rtl_run()
|
||||
@ -79,7 +92,6 @@ void Sub::rtl_climb_start()
|
||||
{
|
||||
rtl_state = RTL_InitialClimb;
|
||||
rtl_state_complete = false;
|
||||
rtl_alt = get_RTL_alt();
|
||||
|
||||
// initialise waypoint and spline controller
|
||||
wp_nav.wp_and_spline_init();
|
||||
@ -89,22 +101,13 @@ void Sub::rtl_climb_start()
|
||||
wp_nav.set_speed_xy(g.rtl_speed_cms);
|
||||
}
|
||||
|
||||
// get horizontal stopping point
|
||||
Vector3f destination;
|
||||
wp_nav.get_wp_stopping_point_xy(destination);
|
||||
|
||||
#if AC_RALLY == ENABLED
|
||||
// rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes
|
||||
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt);
|
||||
rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
|
||||
rally_point.alt = MAX(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home
|
||||
destination.z = pv_alt_above_origin(rally_point.alt);
|
||||
#else
|
||||
destination.z = pv_alt_above_origin(rtl_alt);
|
||||
#endif
|
||||
|
||||
// set the destination
|
||||
wp_nav.set_wp_destination(destination);
|
||||
if (!wp_nav.set_wp_destination(rtl_path.climb_target)) {
|
||||
// this should not happen because rtl_build_path will have checked terrain data was available
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
|
||||
set_mode(LAND, MODE_REASON_TERRAIN_FAILSAFE);
|
||||
return;
|
||||
}
|
||||
wp_nav.set_fast_waypoint(true);
|
||||
|
||||
// hold current yaw during initial climb
|
||||
@ -117,19 +120,10 @@ void Sub::rtl_return_start()
|
||||
rtl_state = RTL_ReturnHome;
|
||||
rtl_state_complete = false;
|
||||
|
||||
// set target to above home/rally point
|
||||
#if AC_RALLY == ENABLED
|
||||
// rally_point will be the nearest rally point or home. uses absolute altitudes
|
||||
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt);
|
||||
rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
|
||||
rally_point.alt = MAX(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home
|
||||
Vector3f destination = pv_location_to_vector(rally_point);
|
||||
#else
|
||||
Vector3f destination = pv_location_to_vector(ahrs.get_home());
|
||||
destination.z = pv_alt_above_origin(rtl_alt);
|
||||
#endif
|
||||
|
||||
wp_nav.set_wp_destination(destination);
|
||||
if (!wp_nav.set_wp_destination(rtl_path.return_target)) {
|
||||
// failure must be caused by missing terrain data, restart RTL
|
||||
rtl_restart_without_terrain();
|
||||
}
|
||||
|
||||
// initialise yaw to point home (maybe)
|
||||
set_auto_yaw_mode(get_default_auto_yaw_mode(true));
|
||||
@ -164,7 +158,7 @@ void Sub::rtl_climb_return_run()
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run waypoint controller
|
||||
wp_nav.update_wpnav();
|
||||
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();
|
||||
@ -182,7 +176,7 @@ void Sub::rtl_climb_return_run()
|
||||
rtl_state_complete = wp_nav.reached_wp_destination();
|
||||
}
|
||||
|
||||
// rtl_return_start - initialise return to home
|
||||
// rtl_loiterathome_start - initialise return to home
|
||||
void Sub::rtl_loiterathome_start()
|
||||
{
|
||||
rtl_state = RTL_LoiterAtHome;
|
||||
@ -226,7 +220,7 @@ void Sub::rtl_loiterathome_run()
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run waypoint controller
|
||||
wp_nav.update_wpnav();
|
||||
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();
|
||||
@ -321,14 +315,14 @@ void Sub::rtl_descent_run()
|
||||
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
// call z-axis position controller
|
||||
pos_control.set_alt_target_with_slew(pv_alt_above_origin(g.rtl_alt_final), G_Dt);
|
||||
pos_control.set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt);
|
||||
pos_control.update_z_controller();
|
||||
|
||||
// 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);
|
||||
|
||||
// check if we've reached within 20cm of final altitude
|
||||
rtl_state_complete = fabsf(pv_alt_above_origin(g.rtl_alt_final) - inertial_nav.get_altitude()) < 20.0f;
|
||||
rtl_state_complete = fabsf(rtl_path.descent_target.alt - current_loc.alt) < 20.0f;
|
||||
}
|
||||
|
||||
// rtl_loiterathome_start - initialise controllers to loiter over home
|
||||
@ -423,21 +417,87 @@ void Sub::rtl_land_run()
|
||||
rtl_state_complete = ap.land_complete;
|
||||
}
|
||||
|
||||
// get_RTL_alt - return altitude which vehicle should return home at
|
||||
// altitude is in cm above home
|
||||
float Sub::get_RTL_alt()
|
||||
void Sub::rtl_build_path(bool terrain_following_allowed)
|
||||
{
|
||||
// origin point is our stopping point
|
||||
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);
|
||||
|
||||
// set return target to nearest rally point or home position
|
||||
#if AC_RALLY == ENABLED
|
||||
rtl_path.return_target = rally.calc_best_rally_or_home_location(current_loc, ahrs.get_home().alt);
|
||||
#else
|
||||
rtl_path.return_target = ahrs.get_home();
|
||||
#endif
|
||||
|
||||
// compute return altitude
|
||||
rtl_compute_return_alt(rtl_path.origin_point, rtl_path.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());
|
||||
|
||||
// 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);
|
||||
|
||||
// set land flag
|
||||
rtl_path.land = g.rtl_alt_final <= 0;
|
||||
}
|
||||
|
||||
// return altitude in cm above home at which vehicle should return home
|
||||
// rtl_origin_point is the stopping point of the vehicle when rtl is initiated
|
||||
// rtl_return_target is the home or rally point that the vehicle is returning to. It's lat, lng and alt values must already have been filled in before this function is called
|
||||
// rtl_return_target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set)
|
||||
void Sub::rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target, bool terrain_following_allowed)
|
||||
{
|
||||
float rtl_return_dist_cm = rtl_return_target.get_distance(rtl_origin_point) * 100.0f;
|
||||
|
||||
// curr_alt is current altitude above home or above terrain depending upon use_terrain
|
||||
int32_t curr_alt = current_loc.alt;
|
||||
|
||||
// decide if we should use terrain altitudes
|
||||
rtl_path.terrain_used = terrain_use() && 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_origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, origin_terr_alt) ||
|
||||
!rtl_origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, return_target_terr_alt) ||
|
||||
!current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) {
|
||||
rtl_path.terrain_used = false;
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
|
||||
}
|
||||
}
|
||||
|
||||
// maximum of current altitude + climb_min and rtl altitude
|
||||
float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), g.rtl_altitude);
|
||||
ret = MAX(ret, RTL_ALT_MIN);
|
||||
float ret = MAX(curr_alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN));
|
||||
|
||||
// don't allow really shallow slopes
|
||||
if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) {
|
||||
ret = MAX(curr_alt, MIN(ret, MAX(rtl_return_dist_cm*g.rtl_cone_slope, curr_alt+RTL_ABS_MIN_CLIMB)));
|
||||
}
|
||||
|
||||
#if AC_FENCE == ENABLED
|
||||
// ensure not above fence altitude if alt fence is enabled
|
||||
// Note: we are assuming the fence alt is the same frame as ret
|
||||
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
|
||||
ret = MIN(ret, fence.get_safe_alt()*100.0f);
|
||||
}
|
||||
#endif
|
||||
|
||||
return ret;
|
||||
// ensure we do not descend
|
||||
ret = MAX(ret, curr_alt);
|
||||
|
||||
// convert return-target to alt-above-home or alt-above-terrain
|
||||
if (!rtl_path.terrain_used || !rtl_return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_TERRAIN)) {
|
||||
if (!rtl_return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME)) {
|
||||
// this should never happen but just in case
|
||||
rtl_return_target.set_alt_cm(0, Location_Class::ALT_FRAME_ABOVE_HOME);
|
||||
}
|
||||
}
|
||||
|
||||
// add ret to altitude
|
||||
rtl_return_target.alt += ret;
|
||||
}
|
||||
|
||||
|
||||
@ -125,7 +125,8 @@ enum mode_reason_t {
|
||||
MODE_REASON_GPS_GLITCH,
|
||||
MODE_REASON_MISSION_END,
|
||||
MODE_REASON_THROTTLE_LAND_ESCAPE,
|
||||
MODE_REASON_FENCE_BREACH
|
||||
MODE_REASON_FENCE_BREACH,
|
||||
MODE_REASON_TERRAIN_FAILSAFE
|
||||
};
|
||||
|
||||
// Tuning enumeration
|
||||
@ -285,6 +286,7 @@ enum ThrowModeState {
|
||||
#define LOG_PARAMTUNE_MSG 0x1F
|
||||
#define LOG_HELI_MSG 0x20
|
||||
#define LOG_PRECLAND_MSG 0x21
|
||||
#define LOG_GUIDEDTARGET_MSG 0x22
|
||||
|
||||
#define MASK_LOG_ATTITUDE_FAST (1<<0)
|
||||
#define MASK_LOG_ATTITUDE_MED (1<<1)
|
||||
@ -391,6 +393,9 @@ enum ThrowModeState {
|
||||
#define ERROR_SUBSYSTEM_BARO 18
|
||||
#define ERROR_SUBSYSTEM_CPU 19
|
||||
#define ERROR_SUBSYSTEM_FAILSAFE_ADSB 20
|
||||
#define ERROR_SUBSYSTEM_TERRAIN 21
|
||||
#define ERROR_SUBSYSTEM_NAVIGATION 22
|
||||
#define ERROR_SUBSYSTEM_FAILSAFE_TERRAIN 23
|
||||
// general error codes
|
||||
#define ERROR_CODE_ERROR_RESOLVED 0
|
||||
#define ERROR_CODE_FAILED_TO_INITIALISE 1
|
||||
@ -409,6 +414,12 @@ enum ThrowModeState {
|
||||
#define ERROR_CODE_CRASH_CHECK_LOSS_OF_CONTROL 2
|
||||
// subsystem specific error codes -- flip
|
||||
#define ERROR_CODE_FLIP_ABANDONED 2
|
||||
// subsystem specific error codes -- terrain
|
||||
#define ERROR_CODE_MISSING_TERRAIN_DATA 2
|
||||
// subsystem specific error codes -- navigation
|
||||
#define ERROR_CODE_FAILED_TO_SET_DESTINATION 2
|
||||
#define ERROR_CODE_RESTARTED_RTL 3
|
||||
#define ERROR_CODE_FAILED_CIRCLE_INIT 4
|
||||
|
||||
// parachute failed to deploy because of low altitude or landed
|
||||
#define ERROR_CODE_PARACHUTE_TOO_LOW 2
|
||||
|
||||
@ -123,6 +123,62 @@ void Sub::failsafe_gcs_off_event(void)
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED);
|
||||
}
|
||||
|
||||
// executes terrain failsafe if data is missing for longer than a few seconds
|
||||
// missing_data should be set to true if the vehicle failed to navigate because of missing data, false if navigation is proceeding successfully
|
||||
void Sub::failsafe_terrain_check()
|
||||
{
|
||||
// trigger with 5 seconds of failures while in AUTO mode
|
||||
bool valid_mode = (control_mode == AUTO || control_mode == GUIDED || control_mode == RTL);
|
||||
bool timeout = (failsafe.terrain_last_failure_ms - failsafe.terrain_first_failure_ms) > FS_TERRAIN_TIMEOUT_MS;
|
||||
bool trigger_event = valid_mode && timeout;
|
||||
|
||||
// check for clearing of event
|
||||
if (trigger_event != failsafe.terrain) {
|
||||
if (trigger_event) {
|
||||
failsafe_terrain_on_event();
|
||||
} else {
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_ERROR_RESOLVED);
|
||||
failsafe.terrain = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// set terrain data status (found or not found)
|
||||
void Sub::failsafe_terrain_set_status(bool data_ok)
|
||||
{
|
||||
uint32_t now = millis();
|
||||
|
||||
// record time of first and latest failures (i.e. duration of failures)
|
||||
if (!data_ok) {
|
||||
failsafe.terrain_last_failure_ms = now;
|
||||
if (failsafe.terrain_first_failure_ms == 0) {
|
||||
failsafe.terrain_first_failure_ms = now;
|
||||
}
|
||||
} else {
|
||||
// failures cleared after 0.1 seconds of persistent successes
|
||||
if (now - failsafe.terrain_last_failure_ms > 100) {
|
||||
failsafe.terrain_last_failure_ms = 0;
|
||||
failsafe.terrain_first_failure_ms = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// terrain failsafe action
|
||||
void Sub::failsafe_terrain_on_event()
|
||||
{
|
||||
failsafe.terrain = true;
|
||||
gcs_send_text(MAV_SEVERITY_CRITICAL,"Failsafe: Terrain data missing");
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_FAILSAFE_OCCURRED);
|
||||
|
||||
if (should_disarm_on_failsafe()) {
|
||||
init_disarm_motors();
|
||||
} else if (control_mode == RTL) {
|
||||
rtl_restart_without_terrain();
|
||||
} else {
|
||||
set_mode_RTL_or_land_with_pause(MODE_REASON_TERRAIN_FAILSAFE);
|
||||
}
|
||||
}
|
||||
|
||||
// set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts
|
||||
// this is always called from a failsafe so we trigger notification to pilot
|
||||
void Sub::set_mode_RTL_or_land_with_pause(mode_reason_t reason)
|
||||
|
||||
@ -7,11 +7,11 @@ void Sub::read_inertia()
|
||||
{
|
||||
// inertial altitude estimates
|
||||
inertial_nav.update(G_Dt);
|
||||
}
|
||||
|
||||
// read_inertial_altitude - pull altitude and climb rate from inertial nav library
|
||||
void Sub::read_inertial_altitude()
|
||||
{
|
||||
// pull position from interial nav library
|
||||
current_loc.lng = inertial_nav.get_longitude();
|
||||
current_loc.lat = inertial_nav.get_latitude();
|
||||
|
||||
// exit immediately if we do not have an altitude estimate
|
||||
if (!inertial_nav.get_filter_status().flags.vert_pos) {
|
||||
return;
|
||||
|
||||
@ -234,8 +234,8 @@ void Sub::init_disarm_motors()
|
||||
gcs_send_text(MAV_SEVERITY_INFO, "Disarming motors");
|
||||
#endif
|
||||
|
||||
// save compass offsets learned by the EKF
|
||||
if (ahrs.use_compass()) {
|
||||
// save compass offsets learned by the EKF if enabled
|
||||
if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) {
|
||||
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
|
||||
Vector3f magOffsets;
|
||||
if (ahrs.getMagOffsets(i, magOffsets)) {
|
||||
|
||||
@ -7,9 +7,6 @@
|
||||
// To-Do - rename and move this function to make it's purpose more clear
|
||||
void Sub::run_nav_updates(void)
|
||||
{
|
||||
// fetch position from inertial navigation
|
||||
calc_position();
|
||||
|
||||
// calculate distance and bearing for reporting and autopilot decisions
|
||||
calc_distance_and_bearing();
|
||||
|
||||
@ -17,14 +14,6 @@ void Sub::run_nav_updates(void)
|
||||
run_autopilot();
|
||||
}
|
||||
|
||||
// calc_position - get lat and lon positions from inertial nav library
|
||||
void Sub::calc_position()
|
||||
{
|
||||
// pull position from interial nav library
|
||||
current_loc.lng = inertial_nav.get_longitude();
|
||||
current_loc.lat = inertial_nav.get_latitude();
|
||||
}
|
||||
|
||||
// calc_distance_and_bearing - calculate distance and bearing to next waypoint and home
|
||||
void Sub::calc_distance_and_bearing()
|
||||
{
|
||||
|
||||
@ -192,7 +192,12 @@ void Sub::init_ardupilot()
|
||||
ahrs.set_optflow(&optflow);
|
||||
#endif
|
||||
|
||||
// initialise position controllers
|
||||
// init Location class
|
||||
Location_Class::set_ahrs(&ahrs);
|
||||
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
|
||||
Location_Class::set_terrain(&terrain);
|
||||
wp_nav.set_terrain(&terrain);
|
||||
#endif
|
||||
pos_control.set_dt(MAIN_LOOP_SECONDS);
|
||||
|
||||
// init the optical flow sensor
|
||||
|
||||
@ -31,9 +31,11 @@ bool Sub::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
|
||||
|
||||
switch(control_mode) {
|
||||
case GUIDED:
|
||||
set_auto_armed(true);
|
||||
guided_takeoff_start(takeoff_alt_cm);
|
||||
return true;
|
||||
if (guided_takeoff_start(takeoff_alt_cm)) {
|
||||
set_auto_armed(true);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
case LOITER:
|
||||
case POSHOLD:
|
||||
case ALT_HOLD:
|
||||
|
||||
Loading…
Reference in New Issue
Block a user