mirror of https://github.com/ArduPilot/ardupilot
Sub: Make changes to match Copter changes
This commit is contained in:
Rustom Jehangir
Andrew Tridgell
parent
accbbce304
commit
814605c461
|
|
@ -744,6 +744,11 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
|
|||
case MSG_MAG_CAL_REPORT:
|
||||
sub.compass.send_mag_cal_report(chan);
|
||||
break;
|
||||
|
||||
case MSG_ADSB_VEHICLE:
|
||||
CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);
|
||||
sub.adsb.send_adsb_vehicle(chan);
|
||||
break;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
|
@ -831,6 +836,15 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
|
|||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[8], 0),
|
||||
|
||||
// @Param: ADSB
|
||||
// @DisplayName: ADSB stream rate to ground station
|
||||
// @Description: ADSB stream rate to ground station
|
||||
// @Units: Hz
|
||||
// @Range: 0 50
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("ADSB", 9, GCS_MAVLINK, streamRates[9], 5),
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
|
@ -971,6 +985,12 @@ GCS_MAVLINK::data_stream_send(void)
|
|||
send_message(MSG_VIBRATION);
|
||||
send_message(MSG_RPM);
|
||||
}
|
||||
|
||||
if (sub.gcs_out_of_time) return;
|
||||
|
||||
if (stream_trigger(STREAM_ADSB)) {
|
||||
send_message(MSG_ADSB_VEHICLE);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -1948,6 +1968,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
break;
|
||||
#endif
|
||||
|
||||
#if AC_FENCE == ENABLED
|
||||
// send or receive fence points with GCS
|
||||
case MAVLINK_MSG_ID_FENCE_POINT: // MAV ID: 160
|
||||
case MAVLINK_MSG_ID_FENCE_FETCH_POINT:
|
||||
sub.fence.handle_msg(chan, msg);
|
||||
break;
|
||||
#endif // AC_FENCE == ENABLED
|
||||
|
||||
#if CAMERA == ENABLED
|
||||
//deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE
|
||||
case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: // MAV ID: 202
|
||||
|
|
|
|||
|
|
@ -301,13 +301,14 @@ struct PACKED log_Control_Tuning {
|
|||
float throttle_in;
|
||||
float angle_boost;
|
||||
float throttle_out;
|
||||
float throttle_hover;
|
||||
float desired_alt;
|
||||
float inav_alt;
|
||||
int32_t baro_alt;
|
||||
int16_t desired_rangefinder_alt;
|
||||
int16_t rangefinder_alt;
|
||||
float terr_alt;
|
||||
int16_t desired_climb_rate;
|
||||
int16_t target_climb_rate;
|
||||
int16_t climb_rate;
|
||||
};
|
||||
|
||||
|
|
@ -326,13 +327,14 @@ void Sub::Log_Write_Control_Tuning()
|
|||
throttle_in : attitude_control.get_throttle_in(),
|
||||
angle_boost : attitude_control.angle_boost(),
|
||||
throttle_out : motors.get_throttle(),
|
||||
throttle_hover : motors.get_throttle_hover(),
|
||||
desired_alt : pos_control.get_alt_target() / 100.0f,
|
||||
inav_alt : inertial_nav.get_altitude() / 100.0f,
|
||||
baro_alt : baro_alt,
|
||||
desired_rangefinder_alt : (int16_t)target_rangefinder_alt,
|
||||
rangefinder_alt : rangefinder_state.alt_cm,
|
||||
terr_alt : terr_alt,
|
||||
desired_climb_rate : (int16_t)pos_control.get_vel_target_z(),
|
||||
target_climb_rate : (int16_t)pos_control.get_vel_target_z(),
|
||||
climb_rate : climb_rate
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
|
|
@ -708,7 +710,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", "Qfffffeccfhh", "TimeUS,ThrIn,ABst,ThrOut,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt" },
|
||||
"CTUN", "Qffffffeccfhh", "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),
|
||||
|
|
@ -825,7 +827,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) {};
|
||||
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() {}
|
||||
|
|
|
|||
|
|
@ -286,15 +286,6 @@ const AP_Param::Info Sub::var_info[] = {
|
|||
// @User: Standard
|
||||
GSCALAR(pilot_accel_z, "PILOT_ACCEL_Z", PILOT_ACCEL_Z_DEFAULT),
|
||||
|
||||
// @Param: THR_MIN
|
||||
// @DisplayName: Throttle Minimum
|
||||
// @Description: The minimum throttle that will be sent to the motors to keep them spinning
|
||||
// @Units: Percent*10
|
||||
// @Range: 0 300
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
GSCALAR(throttle_min, "THR_MIN", THR_MIN_DEFAULT),
|
||||
|
||||
// @Param: FS_THR_ENABLE
|
||||
// @DisplayName: Throttle Failsafe Enable
|
||||
// @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel
|
||||
|
|
@ -311,15 +302,6 @@ const AP_Param::Info Sub::var_info[] = {
|
|||
// @User: Standard
|
||||
GSCALAR(failsafe_throttle_value, "FS_THR_VALUE", FS_THR_VALUE_DEFAULT),
|
||||
|
||||
// @Param: THR_MID
|
||||
// @DisplayName: Throttle Mid Position
|
||||
// @Description: The throttle output (0 ~ 1000) when throttle stick is in mid position. Used to scale the manual throttle so that the mid throttle stick position is close to the throttle required to hover
|
||||
// @User: Standard
|
||||
// @Range: 300 700
|
||||
// @Units: Percent*10
|
||||
// @Increment: 10
|
||||
GSCALAR(throttle_mid, "THR_MID", THR_MID_DEFAULT),
|
||||
|
||||
// @Param: THR_DZ
|
||||
// @DisplayName: Throttle deadzone
|
||||
// @Description: The deadzone above and below mid throttle. Used in AltHold, Loiter, PosHold flight modes
|
||||
|
|
@ -863,7 +845,7 @@ const AP_Param::Info Sub::var_info[] = {
|
|||
GOBJECT(circle_nav, "CIRCLE_", AC_Circle),
|
||||
|
||||
// @Group: ATC_
|
||||
// @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
|
||||
// @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
|
||||
GOBJECT(attitude_control, "ATC_", AC_AttitudeControl_Multi),
|
||||
|
||||
// @Group: PSC
|
||||
|
|
@ -937,6 +919,10 @@ const AP_Param::Info Sub::var_info[] = {
|
|||
GOBJECT(fence, "FENCE_", AC_Fence),
|
||||
#endif
|
||||
|
||||
// @Group: AVOID_
|
||||
// @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp
|
||||
GOBJECT(avoid, "AVOID_", AC_Avoid),
|
||||
|
||||
#if AC_RALLY == ENABLED
|
||||
// @Group: RALLY_
|
||||
// @Path: ../libraries/AP_Rally/AP_Rally.cpp
|
||||
|
|
@ -1042,7 +1028,7 @@ const AP_Param::Info Sub::var_info[] = {
|
|||
// @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
|
||||
// @Values: 0:Do Not Use in RTL and Land,1:Use in RTL and Land
|
||||
// @User: Standard
|
||||
GSCALAR(terrain_follow, "TERRAIN_FOLLOW", 0),
|
||||
|
||||
|
|
@ -1136,6 +1122,10 @@ void Sub::convert_pid_parameters(void)
|
|||
{ Parameters::k_param_pid_rate_pitch, 6, AP_PARAM_FLOAT, "ATC_RAT_PIT_FILT" },
|
||||
{ Parameters::k_param_pid_rate_yaw, 6, AP_PARAM_FLOAT, "ATC_RAT_YAW_FILT" }
|
||||
};
|
||||
AP_Param::ConversionInfo throttle_conversion_info[] = {
|
||||
{ Parameters::k_param_throttle_min, 0, AP_PARAM_FLOAT, "MOT_SPIN_MIN" },
|
||||
{ Parameters::k_param_throttle_mid, 0, AP_PARAM_FLOAT, "MOT_THST_HOVER" }
|
||||
};
|
||||
|
||||
// gains increase by 27% due to attitude controller's switch to use radians instead of centi-degrees
|
||||
// and motor libraries switch to accept inputs in -1 to +1 range instead of -4500 ~ +4500
|
||||
|
|
@ -1161,4 +1151,9 @@ void Sub::convert_pid_parameters(void)
|
|||
for (uint8_t i=0; i<table_size; i++) {
|
||||
AP_Param::convert_old_parameter(&angle_and_filt_conversion_info[i], 1.0f);
|
||||
}
|
||||
// convert throttle parameters (multicopter only)
|
||||
table_size = ARRAY_SIZE(throttle_conversion_info);
|
||||
for (uint8_t i=0; i<table_size; i++) {
|
||||
AP_Param::convert_old_parameter(&throttle_conversion_info[i], 0.001f);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -182,6 +182,7 @@ public:
|
|||
k_param_fs_crash_check,
|
||||
k_param_throw_motor_start,
|
||||
k_param_terrain_follow, // 94
|
||||
k_param_avoid,
|
||||
|
||||
// 97: RSSI
|
||||
k_param_rssi = 97,
|
||||
|
|
@ -281,7 +282,7 @@ public:
|
|||
k_param_rc_8,
|
||||
k_param_rc_10,
|
||||
k_param_rc_11,
|
||||
k_param_throttle_min,
|
||||
k_param_throttle_min, // remove
|
||||
k_param_throttle_max, // remove
|
||||
k_param_failsafe_throttle,
|
||||
k_param_throttle_fs_action, // remove
|
||||
|
|
@ -293,7 +294,7 @@ public:
|
|||
k_param_radio_tuning_low,
|
||||
k_param_rc_speed = 192,
|
||||
k_param_failsafe_battery_enabled,
|
||||
k_param_throttle_mid,
|
||||
k_param_throttle_mid, // remove
|
||||
k_param_failsafe_gps_enabled, // remove
|
||||
k_param_rc_9,
|
||||
k_param_rc_12,
|
||||
|
|
@ -437,10 +438,8 @@ public:
|
|||
|
||||
// Throttle
|
||||
//
|
||||
AP_Int16 throttle_min;
|
||||
AP_Int8 failsafe_throttle;
|
||||
AP_Int16 failsafe_throttle_value;
|
||||
AP_Int16 throttle_mid;
|
||||
AP_Int16 throttle_deadzone;
|
||||
|
||||
// Flight modes
|
||||
|
|
|
|||
|
|
@ -45,7 +45,6 @@ Sub::Sub(void) :
|
|||
super_simple_cos_yaw(1.0),
|
||||
super_simple_sin_yaw(0.0f),
|
||||
initial_armed_bearing(0),
|
||||
throttle_average(0.0f),
|
||||
desired_climb_rate(0),
|
||||
loiter_time_max(0),
|
||||
loiter_time(0),
|
||||
|
|
@ -71,6 +70,7 @@ Sub::Sub(void) :
|
|||
pos_control(ahrs, inertial_nav, motors, attitude_control,
|
||||
g.p_alt_hold, g.p_vel_z, g.pid_accel_z,
|
||||
g.p_pos_xy, g.pi_vel_xy),
|
||||
avoid(ahrs, inertial_nav, fence),
|
||||
wp_nav(inertial_nav, ahrs, pos_control, attitude_control),
|
||||
circle_nav(inertial_nav, ahrs, pos_control),
|
||||
pmTest1(0),
|
||||
|
|
@ -86,7 +86,7 @@ Sub::Sub(void) :
|
|||
camera_mount(ahrs, current_loc),
|
||||
#endif
|
||||
#if AC_FENCE == ENABLED
|
||||
fence(inertial_nav),
|
||||
fence(ahrs, inertial_nav),
|
||||
#endif
|
||||
#if AC_RALLY == ENABLED
|
||||
rally(ahrs),
|
||||
|
|
|
|||
|
|
@ -79,6 +79,7 @@
|
|||
#include <AC_WPNav/AC_Circle.h> // circle navigation library
|
||||
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
|
||||
#include <AC_Fence/AC_Fence.h> // ArduCopter Fence library
|
||||
#include <AC_Avoidance/AC_Avoid.h> // Arducopter stop at fence library
|
||||
#include <AP_Scheduler/AP_Scheduler.h> // main loop scheduler
|
||||
#include <AP_RCMapper/AP_RCMapper.h> // RC input mapping library
|
||||
#include <AP_Notify/AP_Notify.h> // Notify library
|
||||
|
|
@ -87,6 +88,7 @@
|
|||
#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
|
||||
#include <AP_LandingGear/AP_LandingGear.h> // Landing Gear library
|
||||
#include <AP_Terrain/AP_Terrain.h>
|
||||
#include <AP_ADSB/AP_ADSB.h>
|
||||
#include <AP_RPM/AP_RPM.h>
|
||||
#include <AC_InputManager/AC_InputManager.h> // Pilot input handling library
|
||||
#include <AP_JSButton/AP_JSButton.h> // Joystick/gamepad button function assignment
|
||||
|
|
@ -476,6 +478,7 @@ private:
|
|||
AC_AttitudeControl_Multi attitude_control;
|
||||
|
||||
AC_PosControl pos_control;
|
||||
AC_Avoid avoid;
|
||||
AC_WPNav wp_nav;
|
||||
AC_Circle circle_nav;
|
||||
|
||||
|
|
@ -552,6 +555,8 @@ private:
|
|||
AC_PrecLand precland;
|
||||
#endif
|
||||
|
||||
AP_ADSB adsb {ahrs};
|
||||
|
||||
// use this to prevent recursion during sensor init
|
||||
bool in_mavlink_delay;
|
||||
|
||||
|
|
@ -617,7 +622,7 @@ private:
|
|||
void check_ekf_yaw_reset();
|
||||
float get_roi_yaw();
|
||||
float get_look_ahead_yaw();
|
||||
void update_thr_average();
|
||||
void update_throttle_hover();
|
||||
void set_throttle_takeoff();
|
||||
float get_pilot_desired_throttle(int16_t throttle_control);
|
||||
float get_pilot_desired_climb_rate(float throttle_control);
|
||||
|
|
|
|||
|
|
@ -11,5 +11,7 @@ void Sub::init_capabilities(void)
|
|||
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);
|
||||
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
|
||||
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_TERRAIN);
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
|
|
@ -509,14 +509,6 @@
|
|||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Throttle control gains
|
||||
//
|
||||
#ifndef THR_MID_DEFAULT
|
||||
# define THR_MID_DEFAULT 500 // Throttle output (0 ~ 1000) when throttle stick is in mid position
|
||||
#endif
|
||||
|
||||
#ifndef THR_MIN_DEFAULT
|
||||
# define THR_MIN_DEFAULT 0 // minimum throttle sent to the motors when armed and pilot throttle above zero
|
||||
#endif
|
||||
#define THR_MAX 1000 // maximum throttle input and output sent to the motors
|
||||
|
||||
#ifndef THR_DZ_DEFAULT
|
||||
# define THR_DZ_DEFAULT 100 // the deadzone above and below mid throttle while in althold or loiter
|
||||
|
|
|
|||
|
|
@ -174,7 +174,7 @@ void Sub::auto_takeoff_run()
|
|||
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);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}
|
||||
|
||||
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
|
||||
|
|
@ -251,10 +251,10 @@ void Sub::auto_wp_run()
|
|||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain());
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -319,10 +319,10 @@ void Sub::auto_spline_run()
|
|||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true);
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true, get_smoothing_gain());
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -416,7 +416,7 @@ void Sub::auto_land_run()
|
|||
desired_climb_rate = cmb_rate;
|
||||
|
||||
// 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);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}
|
||||
|
||||
// auto_rtl_start - initialises RTL in AUTO flight mode
|
||||
|
|
@ -512,7 +512,7 @@ void Sub::auto_circle_run()
|
|||
pos_control.update_z_controller();
|
||||
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true, get_smoothing_gain());
|
||||
}
|
||||
|
||||
#if NAV_GUIDED == ENABLED
|
||||
|
|
@ -589,7 +589,7 @@ void Sub::auto_loiter_run()
|
|||
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);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}
|
||||
|
||||
// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
|
||||
|
|
|
|||
|
|
@ -44,7 +44,7 @@ void Sub::brake_run()
|
|||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
|
||||
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(0)-throttle_average);
|
||||
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(0)-motors.get_throttle_hover());
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -65,7 +65,7 @@ void Sub::brake_run()
|
|||
wp_nav.update_brake(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f, get_smoothing_gain());
|
||||
|
||||
// body-frame rate controller is run directly from 100hz loop
|
||||
|
||||
|
|
|
|||
|
|
@ -80,9 +80,9 @@ void Sub::circle_run()
|
|||
|
||||
// call attitude controller
|
||||
if (circle_pilot_yaw_override) {
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}else{
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true, get_smoothing_gain());
|
||||
}
|
||||
|
||||
// adjust climb rate using rangefinder
|
||||
|
|
|
|||
|
|
@ -95,7 +95,7 @@ void Sub::drift_run()
|
|||
}
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// output pilot's throttle with angle boost
|
||||
attitude_control.set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt);
|
||||
|
|
|
|||
|
|
@ -177,7 +177,7 @@ void Sub::flip_run()
|
|||
|
||||
case Flip_Recover:
|
||||
// use originally captured earth-frame angle targets to recover
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false);
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false, get_smoothing_gain());
|
||||
|
||||
// increase throttle to gain any lost altitude
|
||||
throttle_out += FLIP_THR_INC;
|
||||
|
|
|
|||
|
|
@ -177,7 +177,8 @@ bool Sub::guided_set_destination(const Vector3f& destination)
|
|||
|
||||
#if AC_FENCE == ENABLED
|
||||
// reject destination if outside the fence
|
||||
if (!fence.check_destination_within_fence(pv_alt_above_home(destination.z)*0.01f, pv_distance_to_home_cm(destination)*0.01f)) {
|
||||
Location_Class dest_loc(destination);
|
||||
if (!fence.check_destination_within_fence(dest_loc)) {
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
|
||||
// failure is propagated to GCS with NAK
|
||||
return false;
|
||||
|
|
@ -205,13 +206,10 @@ bool Sub::guided_set_destination(const Location_Class& dest_loc)
|
|||
#if AC_FENCE == ENABLED
|
||||
// reject destination outside the fence.
|
||||
// Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
|
||||
int32_t alt_target_cm;
|
||||
if (dest_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target_cm)) {
|
||||
if (!fence.check_destination_within_fence(alt_target_cm*0.01f, get_distance_cm(ahrs.get_home(), dest_loc)*0.01f)) {
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
|
||||
// failure is propagated to GCS with NAK
|
||||
return false;
|
||||
}
|
||||
if (!fence.check_destination_within_fence(dest_loc)) {
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
|
||||
// failure is propagated to GCS with NAK
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -337,7 +335,7 @@ void Sub::guided_takeoff_run()
|
|||
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);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}
|
||||
|
||||
// guided_pos_control_run - runs the guided position controller
|
||||
|
|
@ -375,10 +373,10 @@ void Sub::guided_pos_control_run()
|
|||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true);
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true, get_smoothing_gain());
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -422,10 +420,10 @@ void Sub::guided_vel_control_run()
|
|||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true);
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true, get_smoothing_gain());
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -491,10 +489,10 @@ void Sub::guided_posvel_control_run()
|
|||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true);
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true, get_smoothing_gain());
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -527,7 +525,7 @@ void Sub::guided_angle_control_run()
|
|||
float yaw_in = wrap_180_cd_float(guided_angle_state.yaw_cd);
|
||||
|
||||
// constrain climb rate
|
||||
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabs(wp_nav.get_speed_down()), wp_nav.get_speed_up());
|
||||
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav.get_speed_down()), wp_nav.get_speed_up());
|
||||
|
||||
// check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
|
||||
uint32_t tnow = millis();
|
||||
|
|
@ -541,7 +539,7 @@ void Sub::guided_angle_control_run()
|
|||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true, get_smoothing_gain());
|
||||
|
||||
// call position controller
|
||||
pos_control.set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);
|
||||
|
|
|
|||
|
|
@ -127,7 +127,7 @@ void Sub::land_gps_run()
|
|||
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// pause 4 seconds before beginning land descent
|
||||
float cmb_rate;
|
||||
|
|
@ -192,7 +192,7 @@ void Sub::land_nogps_run()
|
|||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// pause 4 seconds before beginning land descent
|
||||
float cmb_rate;
|
||||
|
|
|
|||
|
|
@ -166,10 +166,10 @@ void Sub::rtl_climb_return_run()
|
|||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain());
|
||||
}
|
||||
|
||||
// check if we've completed this stage of RTL
|
||||
|
|
@ -228,10 +228,10 @@ void Sub::rtl_loiterathome_run()
|
|||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain());
|
||||
}
|
||||
|
||||
// check if we've completed this stage of RTL
|
||||
|
|
@ -319,7 +319,7 @@ void Sub::rtl_descent_run()
|
|||
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);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// check if we've reached within 20cm of final altitude
|
||||
rtl_state_complete = fabsf(rtl_path.descent_target.alt - current_loc.alt) < 20.0f;
|
||||
|
|
@ -411,7 +411,7 @@ void Sub::rtl_land_run()
|
|||
desired_climb_rate = cmb_rate;
|
||||
|
||||
// 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);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// check if we've completed this stage of RTL
|
||||
rtl_state_complete = ap.land_complete;
|
||||
|
|
|
|||
|
|
@ -51,7 +51,7 @@ void Sub::stabilize_run()
|
|||
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in);
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// body-frame rate controller is run directly from 100hz loop
|
||||
|
||||
|
|
|
|||
|
|
@ -140,7 +140,7 @@ void Sub::throw_run()
|
|||
case Throw_Uprighting:
|
||||
|
||||
// demand a level roll/pitch attitude with zero yaw rate
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
|
||||
|
||||
// output 50% throttle and turn off angle boost to maximise righting moment
|
||||
attitude_control.set_throttle_out(500, false, g.throttle_filt);
|
||||
|
|
@ -150,7 +150,7 @@ void Sub::throw_run()
|
|||
case Throw_HgtStabilise:
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
|
||||
|
||||
// call height controller
|
||||
pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
|
||||
|
|
@ -164,7 +164,7 @@ void Sub::throw_run()
|
|||
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f, get_smoothing_gain());
|
||||
|
||||
// call height controller
|
||||
pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
|
||||
|
|
|
|||
|
|
@ -360,10 +360,11 @@ enum ThrowModeState {
|
|||
#define DATA_ROTOR_SPEED_BELOW_CRITICAL 59 // Heli only
|
||||
#define DATA_EKF_ALT_RESET 60
|
||||
#define DATA_LAND_CANCELLED_BY_PILOT 61
|
||||
#define DATA_SURFACED 62 // Sub only
|
||||
#define DATA_NOT_SURFACED 63 // Sub only
|
||||
#define DATA_BOTTOMED 64 // Sub only
|
||||
#define DATA_NOT_BOTTOMED 65 // Sub only
|
||||
#define DATA_EKF_YAW_RESET 62
|
||||
#define DATA_SURFACED 63 // Sub only
|
||||
#define DATA_NOT_SURFACED 64 // Sub only
|
||||
#define DATA_BOTTOMED 65 // Sub only
|
||||
#define DATA_NOT_BOTTOMED 66 // Sub only
|
||||
|
||||
// Centi-degrees to radians
|
||||
#define DEGX100 5729.57795f
|
||||
|
|
|
|||
|
|
@ -3,6 +3,11 @@
|
|||
#include "Sub.h"
|
||||
#define LAND_END_DEPTH -20 //Landed at 20cm depth
|
||||
|
||||
// Code to detect a crash main ArduCopter code
|
||||
#define LAND_CHECK_ANGLE_ERROR_DEG 30.0f // maximum angle error to be considered landing
|
||||
#define LAND_CHECK_LARGE_ANGLE_CD 1500.0f // maximum angle target to be considered landing
|
||||
#define LAND_CHECK_ACCEL_MOVING 3.0f // maximum acceleration after subtracting gravity
|
||||
|
||||
// counter to verify landings
|
||||
static uint32_t land_detector_count = 0;
|
||||
|
||||
|
|
|
|||
|
|
@ -43,6 +43,7 @@ LIBRARIES += AC_WPNav
|
|||
LIBRARIES += AC_Circle
|
||||
LIBRARIES += AP_Declination
|
||||
LIBRARIES += AC_Fence
|
||||
LIBRARIES += AC_Avoidance
|
||||
LIBRARIES += AP_Scheduler
|
||||
LIBRARIES += AP_RCMapper
|
||||
LIBRARIES += AP_Notify
|
||||
|
|
@ -58,4 +59,5 @@ LIBRARIES += AP_RPM
|
|||
LIBRARIES += AC_PrecLand
|
||||
LIBRARIES += AP_IRLock
|
||||
LIBRARIES += AC_InputManager
|
||||
LIBRARIES += AP_ADSB
|
||||
LIBRARIES += AP_JSButton
|
||||
|
|
|
|||
|
|
@ -191,6 +191,8 @@ void Sub::init_ardupilot()
|
|||
Location_Class::set_terrain(&terrain);
|
||||
wp_nav.set_terrain(&terrain);
|
||||
#endif
|
||||
wp_nav.set_avoidance(&avoid);
|
||||
|
||||
pos_control.set_dt(MAIN_LOOP_SECONDS);
|
||||
|
||||
// init the optical flow sensor
|
||||
|
|
|
|||
|
|
@ -7,9 +7,11 @@ def build(bld):
|
|||
name=vehicle + '_libs',
|
||||
vehicle=vehicle,
|
||||
libraries=bld.ap_common_vehicle_libraries() + [
|
||||
'AP_ADSB',
|
||||
'AC_AttitudeControl',
|
||||
'AC_InputManager',
|
||||
'AC_Fence',
|
||||
'AC_Avoidance',
|
||||
'AC_PID',
|
||||
'AC_PrecLand',
|
||||
'AC_Sprayer',
|
||||
|
|
|
|||
Loading…
Reference in New Issue