Sub: Match Copter changes

This commit is contained in:
Rustom Jehangir 2016-05-02 21:45:37 -07:00 committed by Andrew Tridgell
parent 37118920ed
commit 617b439d0e
23 changed files with 648 additions and 265 deletions

View File

@ -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

View File

@ -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;

View File

@ -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() {}

View File

@ -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
};

View File

@ -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;

View File

@ -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),

View File

@ -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);

View File

@ -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

View File

@ -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);
}

View File

@ -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);

View File

@ -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");

View File

@ -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

View File

@ -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);
}

View File

@ -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();

View File

@ -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);

View File

@ -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;
}

View File

@ -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

View File

@ -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)

View File

@ -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;

View File

@ -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)) {

View File

@ -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()
{

View File

@ -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

View File

@ -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: