aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Sub: Remove heli support from sub.

Browse Source
This commit is contained in:
Rustom Jehangir 2016-02-22 22:15:15 -08:00 committed by Andrew Tridgell
parent 105eae0e86
commit 6cf24c2770
30 changed files with 58 additions and 908 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduSub/APM_Config.h
View File

@ -7,15 +7,6 @@
//#define FRAME_CONFIG VECTORED_FRAME
/* options:
* QUAD_FRAME
* TRI_FRAME
* HEXA_FRAME
* Y6_FRAME
* OCTA_FRAME
* OCTA_QUAD_FRAME
* HELI_FRAME
* SINGLE_FRAME
* COAX_FRAME
* BLUEROV_FRAME
* VECTORED_FRAME
*/

5
ArduSub/AP_State.cpp
View File

@ -116,13 +116,8 @@ void Sub::set_pre_arm_rc_check(bool b)
void Sub::update_using_interlock()
{
#if FRAME_CONFIG == HELI_FRAME
// helicopters are always using motor interlock
ap.using_interlock = true;
#else
// check if we are using motor interlock control on an aux switch
ap.using_interlock = check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK);
#endif
}
void Sub::set_motor_emergency_stop(bool b)

5
ArduSub/Attitude.cpp
View File

@ -225,13 +225,8 @@ float Sub::get_throttle_pre_takeoff(float input_thr)
float in_min = g.throttle_min;
float in_max = get_takeoff_trigger_throttle();
#if FRAME_CONFIG == HELI_FRAME
// helicopters swash will move from bottom to 1/2 of mid throttle
float out_min = 0;
#else
// multicopters will output between spin-when-armed and 1/2 of mid throttle
float out_min = motors.get_throttle_warn();
#endif
float out_max = get_non_takeoff_throttle();
if ((g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0) {

20
ArduSub/Log.cpp
View File

@ -429,7 +429,6 @@ struct PACKED log_MotBatt {
// Write an rate packet
void Sub::Log_Write_MotBatt()
{
#if FRAME_CONFIG != HELI_FRAME
struct log_MotBatt pkt_mot = {
LOG_PACKET_HEADER_INIT(LOG_MOTBATT_MSG),
time_us : AP_HAL::micros64(),
@ -439,7 +438,6 @@ void Sub::Log_Write_MotBatt()
th_limit : (float)(motors.get_throttle_limit())
};
DataFlash.WriteBlock(&pkt_mot, sizeof(pkt_mot));
#endif
}
struct PACKED log_Startup {
@ -671,20 +669,6 @@ struct PACKED log_Heli {
int16_t main_rotor_speed;
};
#if FRAME_CONFIG == HELI_FRAME
// Write an helicopter packet
void Sub::Log_Write_Heli()
{
struct log_Heli pkt_heli = {
LOG_PACKET_HEADER_INIT(LOG_HELI_MSG),
time_us : AP_HAL::micros64(),
desired_rotor_speed : motors.get_desired_rotor_speed(),
main_rotor_speed : motors.get_main_rotor_speed(),
};
DataFlash.WriteBlock(&pkt_heli, sizeof(pkt_heli));
}
#endif
// precision landing logging
struct PACKED log_Precland {
LOG_PACKET_HEADER;
@ -861,10 +845,6 @@ void Sub::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t co
void Sub::Log_Write_Home_And_Origin() {}
void Sub::Log_Sensor_Health() {}
#if FRAME_CONFIG == HELI_FRAME
void Sub::Log_Write_Heli() {}
#endif
#if OPTFLOW == ENABLED
void Sub::Log_Write_Optflow() {}
#endif

40
ArduSub/Parameters.cpp
View File

@ -511,24 +511,6 @@ const AP_Param::Info Sub::var_info[] = {
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
GSCALAR(fs_crash_check, "FS_CRASH_CHECK", 1),
#if FRAME_CONFIG == HELI_FRAME
// @Group: HS1_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(heli_servo_1, "HS1_", RC_Channel),
// @Group: HS2_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(heli_servo_2, "HS2_", RC_Channel),
// @Group: HS3_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(heli_servo_3, "HS3_", RC_Channel),
// @Group: HS4_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(heli_servo_4, "HS4_", RC_Channel),
// @Group: H_RSC_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(heli_servo_rsc, "H_RSC_", RC_Channel),
#endif
// RC channel
//-----------
@ -668,11 +650,7 @@ const AP_Param::Info Sub::var_info[] = {
// @Range: 0.001 0.02
// @Increment: 0.001
// @User: Standard
#if FRAME_CONFIG == HELI_FRAME
GGROUP(pid_rate_roll, "RATE_RLL_", AC_HELI_PID),
#else
GGROUP(pid_rate_roll, "RATE_RLL_", AC_PID),
#endif
// @Param: RATE_PIT_P
// @DisplayName: Pitch axis rate controller P gain
@ -702,11 +680,7 @@ const AP_Param::Info Sub::var_info[] = {
// @Range: 0.001 0.02
// @Increment: 0.001
// @User: Standard
#if FRAME_CONFIG == HELI_FRAME
GGROUP(pid_rate_pitch, "RATE_PIT_", AC_HELI_PID),
#else
GGROUP(pid_rate_pitch, "RATE_PIT_", AC_PID),
#endif
// @Param: RATE_YAW_P
// @DisplayName: Yaw axis rate controller P gain
@ -736,11 +710,7 @@ const AP_Param::Info Sub::var_info[] = {
// @Range: 0.000 0.02
// @Increment: 0.001
// @User: Standard
#if FRAME_CONFIG == HELI_FRAME
GGROUP(pid_rate_yaw, "RATE_YAW_", AC_HELI_PID),
#else
GGROUP(pid_rate_yaw, "RATE_YAW_", AC_PID),
#endif
// @Param: VEL_XY_P
// @DisplayName: Velocity (horizontal) P gain
@ -892,12 +862,6 @@ const AP_Param::Info Sub::var_info[] = {
// @Path: ../libraries/AP_LandingGear/AP_LandingGear.cpp
GOBJECT(landinggear, "LGR_", AP_LandingGear),
#if FRAME_CONFIG == HELI_FRAME
// @Group: IM_
// @Path: ../libraries/AC_InputManager/AC_InputManager_Heli.cpp
GOBJECT(input_manager, "IM_", AC_InputManager_Heli),
#endif
// @Group: COMPASS_
// @Path: ../libraries/AP_Compass/Compass.cpp
GOBJECT(compass, "COMPASS_", Compass),
@ -914,13 +878,9 @@ const AP_Param::Info Sub::var_info[] = {
// @Path: ../libraries/AC_WPNav/AC_Circle.cpp
GOBJECT(circle_nav, "CIRCLE_", AC_Circle),
#if FRAME_CONFIG == HELI_FRAME
GOBJECT(attitude_control, "ATC_", AC_AttitudeControl_Heli),
#else
// @Group: ATC_
// @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
GOBJECT(attitude_control, "ATC_", AC_AttitudeControl),
#endif
// @Group: POSCON_
// @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp

24
ArduSub/Parameters.h
View File

@ -448,11 +448,6 @@ public:
AP_Float fs_ekf_thresh;
AP_Int16 gcs_pid_mask;
#if FRAME_CONFIG == HELI_FRAME
// Heli
RC_Channel heli_servo_1, heli_servo_2, heli_servo_3, heli_servo_4; // servos for swash plate and tail
RC_Channel heli_servo_rsc; // servo for rotor speed control output
#endif
#if FRAME_CONFIG == SINGLE_FRAME
// Single
RC_Channel single_servo_1, single_servo_2, single_servo_3, single_servo_4; // servos for four flaps
@ -494,15 +489,9 @@ public:
AP_Float acro_expo;
// PI/D controllers
#if FRAME_CONFIG == HELI_FRAME
AC_HELI_PID pid_rate_roll;
AC_HELI_PID pid_rate_pitch;
AC_HELI_PID pid_rate_yaw;
#else
AC_PID pid_rate_roll;
AC_PID pid_rate_pitch;
AC_PID pid_rate_yaw;
#endif
AC_PI_2D pi_vel_xy;
AC_P p_vel_z;
@ -527,13 +516,6 @@ public:
// above.
Parameters() :
#if FRAME_CONFIG == HELI_FRAME
heli_servo_1 (CH_1),
heli_servo_2 (CH_2),
heli_servo_3 (CH_3),
heli_servo_4 (CH_4),
heli_servo_rsc (CH_8),
#endif
#if FRAME_CONFIG == SINGLE_FRAME
single_servo_1 (CH_1),
single_servo_2 (CH_2),
@ -567,15 +549,9 @@ public:
// PID controller initial P initial I initial D initial imax initial filt hz pid rate
//---------------------------------------------------------------------------------------------------------------------------------
#if FRAME_CONFIG == HELI_FRAME
pid_rate_roll (RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_D, RATE_ROLL_IMAX, RATE_ROLL_FILT_HZ, MAIN_LOOP_SECONDS, RATE_ROLL_FF),
pid_rate_pitch (RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_D, RATE_PITCH_IMAX, RATE_PITCH_FILT_HZ, MAIN_LOOP_SECONDS, RATE_PITCH_FF),
pid_rate_yaw (RATE_YAW_P, RATE_YAW_I, RATE_YAW_D, RATE_YAW_IMAX, RATE_YAW_FILT_HZ, MAIN_LOOP_SECONDS, RATE_YAW_FF),
#else
pid_rate_roll (RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_D, RATE_ROLL_IMAX, RATE_ROLL_FILT_HZ, MAIN_LOOP_SECONDS),
pid_rate_pitch (RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_D, RATE_PITCH_IMAX, RATE_PITCH_FILT_HZ, MAIN_LOOP_SECONDS),
pid_rate_yaw (RATE_YAW_P, RATE_YAW_I, RATE_YAW_D, RATE_YAW_IMAX, RATE_YAW_FILT_HZ, MAIN_LOOP_SECONDS),
#endif
pi_vel_xy (VEL_XY_P, VEL_XY_I, VEL_XY_IMAX, VEL_XY_FILT_HZ, WPNAV_LOITER_UPDATE_TIME),

8
ArduSub/Sub.cpp
View File

@ -29,9 +29,7 @@ Sub::Sub(void) :
FUNCTOR_BIND_MEMBER(&Sub::verify_command_callback, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Sub::exit_mission, void)),
control_mode(STABILIZE),
#if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments
motors(g.rc_7, g.heli_servo_rsc, g.heli_servo_1, g.heli_servo_2, g.heli_servo_3, g.heli_servo_4, MAIN_LOOP_RATE),
#elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing
#if FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing
motors(MAIN_LOOP_RATE),
#elif FRAME_CONFIG == SINGLE_FRAME // single constructor requires extra servos for flaps
motors(g.single_servo_1, g.single_servo_2, g.single_servo_3, g.single_servo_4, MAIN_LOOP_RATE),
@ -116,10 +114,6 @@ Sub::Sub(void) :
#endif
#if PRECISION_LANDING == ENABLED
precland(ahrs, inertial_nav, g.pi_precland, MAIN_LOOP_SECONDS),
#endif
#if FRAME_CONFIG == HELI_FRAME
// ToDo: Input Manager is only used by Heli for 3.3, but will be used by all frames for 3.4
input_manager(MAIN_LOOP_RATE),
#endif
in_mavlink_delay(false),
gcs_out_of_time(false),

42
ArduSub/Sub.h
View File

@ -59,10 +59,8 @@
#include <AP_Rally/AP_Rally.h> // Rally point library
#include <AC_PID/AC_PID.h> // PID library
#include <AC_PID/AC_PI_2D.h> // PID library (2-axis)
#include <AC_PID/AC_HELI_PID.h> // Heli specific Rate PID library
#include <AC_PID/AC_P.h> // P library
#include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> // Attitude control library
#include <AC_AttitudeControl/AC_AttitudeControl_Heli.h> // Attitude control library for traditional helicopter
#include <AC_AttitudeControl/AC_PosControl.h> // Position control library
#include <RC_Channel/RC_Channel.h> // RC Channel Library
#include <AP_Motors/AP_Motors.h> // AP Motors library
@ -92,7 +90,6 @@
#include <AP_Terrain/AP_Terrain.h>
#include <AP_RPM/AP_RPM.h>
#include <AC_InputManager/AC_InputManager.h> // Pilot input handling library
#include <AC_InputManager/AC_InputManager_Heli.h> // Heli specific pilot input handling library
// Configuration
#include "defines.h"
@ -428,11 +425,8 @@ private:
// Attitude, Position and Waypoint navigation objects
// To-Do: move inertial nav up or other navigation variables down here
#if FRAME_CONFIG == HELI_FRAME
AC_AttitudeControl_Heli attitude_control;
#else
AC_AttitudeControl_Multi attitude_control;
#endif
AC_PosControl pos_control;
AC_WPNav wp_nav;
AC_Circle circle_nav;
@ -510,13 +504,6 @@ private:
AC_PrecLand precland;
#endif
// Pilot Input Management Library
// Only used for Helicopter for AC3.3, to be expanded to include Multirotor
// child class for AC3.4
#if FRAME_CONFIG == HELI_FRAME
AC_InputManager_Heli input_manager;
#endif
// use this to prevent recursion during sensor init
bool in_mavlink_delay;
@ -527,21 +514,6 @@ private:
// setup the var_info table
AP_Param param_loader;
#if FRAME_CONFIG == HELI_FRAME
// Mode filter to reject RC Input glitches. Filter size is 5, and it draws the 4th element, so it can reject 3 low glitches,
// and 1 high glitch. This is because any "off" glitches can be highly problematic for a helicopter running an ESC
// governor. Even a single "off" frame can cause the rotor to slow dramatically and take a long time to restart.
ModeFilterInt16_Size5 rotor_speed_deglitch_filter {4};
int16_t rsc_control_deglitched;
// Tradheli flags
struct {
uint8_t dynamic_flight : 1; // 0 // true if we are moving at a significant speed (used to turn on/off leaky I terms)
uint8_t init_targets_on_arming : 1; // 1 // true if we have been disarmed, and need to reset rate controller targets when we arm
} heli_flags;
#endif
#if GNDEFFECT_COMPENSATION == ENABLED
// ground effect detector
struct {
@ -655,9 +627,6 @@ private:
void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high);
void Log_Write_Home_And_Origin();
void Log_Sensor_Health();
#if FRAME_CONFIG == HELI_FRAME
void Log_Write_Heli(void);
#endif
void Log_Write_Precland();
void Log_Write_Vehicle_Startup_Messages();
void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
@ -839,16 +808,7 @@ private:
bool mode_has_manual_throttle(uint8_t mode);
bool mode_allows_arming(uint8_t mode, bool arming_from_gcs);
void notify_flight_mode(uint8_t mode);
void heli_init();
void check_dynamic_flight(void);
void update_heli_control_dynamics(void);
void heli_update_landing_swash();
void heli_update_rotor_speed_targets();
void heli_radio_passthrough();
bool heli_acro_init(bool ignore_checks);
void heli_acro_run();
bool heli_stabilize_init(bool ignore_checks);
void heli_stabilize_run();
void read_inertia();
void read_inertial_altitude();
bool land_complete_maybe();

22
ArduSub/arming_checks.cpp
View File

@ -318,12 +318,6 @@ bool Sub::pre_arm_checks(bool display_failure)
return false;
}
#endif
#if FRAME_CONFIG == HELI_FRAME
// check helicopter parameters
if (!motors.parameter_check(display_failure)) {
return false;
}
#endif // HELI_FRAME
}
// check throttle is above failsafe throttle
@ -331,11 +325,7 @@ bool Sub::pre_arm_checks(bool display_failure)
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) {
if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->radio_in < g.failsafe_throttle_value) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Collective below Failsafe");
#else
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Throttle below Failsafe");
#endif
}
return false;
}
@ -626,11 +616,7 @@ bool Sub::arm_checks(bool display_failure, bool arming_from_gcs)
// check throttle is not too low - must be above failsafe throttle
if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->radio_in < g.failsafe_throttle_value) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective below Failsafe");
#else
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle below Failsafe");
#endif
}
return false;
}
@ -640,22 +626,14 @@ bool Sub::arm_checks(bool display_failure, bool arming_from_gcs)
// above top of deadband is too always high
if (channel_throttle->control_in > get_takeoff_trigger_throttle()) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high");
#else
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high");
#endif
}
return false;
}
// in manual modes throttle must be at zero
if ((mode_has_manual_throttle(control_mode) || control_mode == DRIFT) && channel_throttle->control_in > 0) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high");
#else
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high");
#endif
}
return false;
}

5
ArduSub/compassmot.cpp
View File

@ -9,10 +9,6 @@
// setup_compassmot - sets compass's motor interference parameters
uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan)
{
#if FRAME_CONFIG == HELI_FRAME
// compassmot not implemented for tradheli
return 1;
#else
int8_t comp_type; // throttle or current based compensation
Vector3f compass_base[COMPASS_MAX_INSTANCES]; // compass vector when throttle is zero
Vector3f motor_impact[COMPASS_MAX_INSTANCES]; // impact of motors on compass vector
@ -269,6 +265,5 @@ uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan)
ap.compass_mot = false;
return 0;
#endif // FRAME_CONFIG != HELI_FRAME
}

69
ArduSub/config.h
View File

@ -66,25 +66,7 @@
# define FRAME_CONFIG BLUEROV_FRAME
#endif
#if FRAME_CONFIG == QUAD_FRAME
# define FRAME_CONFIG_STRING "QUAD"
#elif FRAME_CONFIG == TRI_FRAME
# define FRAME_CONFIG_STRING "TRI"
#elif FRAME_CONFIG == HEXA_FRAME
# define FRAME_CONFIG_STRING "HEXA"
#elif FRAME_CONFIG == Y6_FRAME
# define FRAME_CONFIG_STRING "Y6"
#elif FRAME_CONFIG == OCTA_FRAME
# define FRAME_CONFIG_STRING "OCTA"
#elif FRAME_CONFIG == OCTA_QUAD_FRAME
# define FRAME_CONFIG_STRING "OCTA_QUAD"
#elif FRAME_CONFIG == HELI_FRAME
# define FRAME_CONFIG_STRING "HELI"
#elif FRAME_CONFIG == SINGLE_FRAME
# define FRAME_CONFIG_STRING "SINGLE"
#elif FRAME_CONFIG == COAX_FRAME
# define FRAME_CONFIG_STRING "COAX"
#elif FRAME_CONFIG == BLUEROV_FRAME
#if FRAME_CONFIG == BLUEROV_FRAME
# define FRAME_CONFIG_STRING "ROV_BLUEROV_FRAME"
#elif FRAME_CONFIG == VECTORED_FRAME
# define FRAME_CONFIG_STRING "ROV_VECTORED_FRAME"
@ -92,49 +74,18 @@
# define FRAME_CONFIG_STRING "UNKNOWN"
#endif
/////////////////////////////////////////////////////////////////////////////////
// TradHeli defaults
#if FRAME_CONFIG == HELI_FRAME
# define RC_FAST_SPEED 125
# define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_LOOK_AHEAD
# define RATE_ROLL_P 0.02
# define RATE_ROLL_I 0.5
# define RATE_ROLL_D 0.001
# define RATE_ROLL_IMAX 4500
# define RATE_ROLL_FF 0.05
# define RATE_ROLL_FILT_HZ 20.0f
# define RATE_PITCH_P 0.02
# define RATE_PITCH_I 0.5
# define RATE_PITCH_D 0.001
# define RATE_PITCH_IMAX 4500
# define RATE_PITCH_FF 0.05
# define RATE_PITCH_FILT_HZ 20.0f
# define RATE_YAW_P 0.15
# define RATE_YAW_I 0.100
# define RATE_YAW_D 0.003
# define RATE_YAW_IMAX 4500
# define RATE_YAW_FF 0.02
# define RATE_YAW_FILT_HZ 20.0f
# define THR_MIN_DEFAULT 0
# define AUTOTUNE_ENABLED DISABLED
#endif
/////////////////////////////////////////////////////////////////////////////////
// Y6 defaults
#if FRAME_CONFIG == Y6_FRAME
# define RATE_ROLL_P 0.1f
# define RATE_ROLL_D 0.006f
# define RATE_PITCH_P 0.1f
# define RATE_PITCH_D 0.006f
# define RATE_YAW_P 0.150f
# define RATE_YAW_I 0.015f
#endif
/////////////////////////////////////////////////////////////////////////////////
// Tri defaults
#if FRAME_CONFIG == TRI_FRAME
# define RATE_YAW_FILT_HZ 100.0f
#endif
//#if FRAME_CONFIG == Y6_FRAME
// # define RATE_ROLL_P 0.1f
// # define RATE_ROLL_D 0.006f
// # define RATE_PITCH_P 0.1f
// # define RATE_PITCH_D 0.006f
// # define RATE_YAW_P 0.150f
// # define RATE_YAW_I 0.015f
//#endif
//////////////////////////////////////////////////////////////////////////////
// PWM control

44
ArduSub/control_althold.cpp
View File

@ -9,13 +9,6 @@
// althold_init - initialise althold controller
bool Sub::althold_init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter Alt Hold if the Rotor Runup is not complete
if (!ignore_checks && !motors.rotor_runup_complete()){
return false;
}
#endif
// initialize vertical speeds and leash lengths
// sets the maximum speed up and down returned by position controller
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
@ -56,12 +49,7 @@ void Sub::althold_run()
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in);
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
#if FRAME_CONFIG == HELI_FRAME
// helicopters are held on the ground until rotor speed runup has finished
bool takeoff_triggered = (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()) && motors.rotor_runup_complete());
#else
bool takeoff_triggered = (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()));
#endif
// // Alt Hold State Machine Determination
// if(!ap.auto_armed) {
@ -80,32 +68,16 @@ void Sub::althold_run()
switch (althold_state) {
case AltHold_Disarmed:
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
attitude_control.set_yaw_target_to_current_heading();
// 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.set_throttle_out(0,false,g.throttle_filt);
#else // Multicopter do not stabilize roll/pitch/yaw when disarmed
// Multicopter do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif // HELI_FRAME
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
break;
case AltHold_MotorStop:
#if FRAME_CONFIG == HELI_FRAME
// helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// force descent rate and call position controller
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
pos_control.update_z_controller();
#else // Multicopter do not stabilize roll/pitch/yaw when motor are stopped
// Multicopter do not stabilize roll/pitch/yaw when motor are stopped
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
#endif // HELI_FRAME
break;
case AltHold_Takeoff:
@ -132,15 +104,9 @@ void Sub::althold_run()
break;
case AltHold_Landed:
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
attitude_control.set_yaw_target_to_current_heading();
// 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.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt);
#else // Multicopter do not stabilize roll/pitch/yaw when disarmed
// Multicopter do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
break;

38
ArduSub/control_auto.cpp
View File

@ -115,14 +115,9 @@ void Sub::auto_takeoff_run()
if(!ap.auto_armed || !motors.get_interlock()) {
// initialise wpnav targets
wp_nav.shift_wp_origin_to_current_pos();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// clear i term when we're taking off
set_throttle_takeoff();
return;
@ -168,13 +163,10 @@ void Sub::auto_wp_run()
if(!ap.auto_armed || !motors.get_interlock()) {
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off)
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// clear i term when we're taking off
set_throttle_takeoff();
return;
@ -232,13 +224,9 @@ void Sub::auto_spline_run()
if(!ap.auto_armed || !motors.get_interlock()) {
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off)
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// clear i term when we're taking off
set_throttle_takeoff();
return;
@ -305,13 +293,9 @@ void Sub::auto_land_run()
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// set target to current position
wp_nav.init_loiter_target();
return;
@ -477,13 +461,9 @@ void Sub::auto_loiter_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
}

8
ArduSub/control_brake.cpp
View File

@ -38,13 +38,9 @@ void Sub::brake_run()
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE);
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(0)-throttle_average);
return;
}

8
ArduSub/control_circle.cpp
View File

@ -44,13 +44,9 @@ void Sub::circle_run()
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
// To-Do: add some initialisation of position controllers
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
pos_control.set_alt_target_to_current_alt();
return;
}

41
ArduSub/control_guided.cpp
View File

@ -252,13 +252,9 @@ void Sub::guided_takeoff_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
}
@ -285,13 +281,9 @@ void Sub::guided_pos_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
}
@ -329,13 +321,9 @@ void Sub::guided_vel_control_run()
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
// initialise velocity controller
pos_control.init_vel_controller_xyz();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
}
@ -377,13 +365,9 @@ void Sub::guided_posvel_control_run()
// set target position and velocity to current position and velocity
pos_control.set_pos_target(inertial_nav.get_position());
pos_control.set_desired_velocity(Vector3f(0,0,0));
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
}
@ -443,14 +427,9 @@ void Sub::guided_angle_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.set_yaw_target_to_current_heading();
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0.0f, 0.0f, 0.0f, get_smoothing_gain());
attitude_control.set_throttle_out(0.0f,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0.0f,true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(0.0f);
return;
}

15
ArduSub/control_land.cpp
View File

@ -57,13 +57,9 @@ void Sub::land_gps_run()
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
wp_nav.init_loiter_target();
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
@ -162,13 +158,8 @@ void Sub::land_nogps_run()
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// 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.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
// disarm when the landing detector says we've landed and throttle is at minimum

37
ArduSub/control_loiter.cpp
View File

@ -9,13 +9,6 @@
// loiter_init - initialise loiter controller
bool Sub::loiter_init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter Loiter if the Rotor Runup is not complete
if (!ignore_checks && !motors.rotor_runup_complete()){
return false;
}
#endif
if (position_ok() || ignore_checks) {
// set target to current position
@ -89,37 +82,18 @@ void Sub::loiter_run()
switch (loiter_state) {
case Loiter_Disarmed:
wp_nav.init_loiter_target();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif // HELI_FRAME
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
break;
case Loiter_MotorStop:
#if FRAME_CONFIG == HELI_FRAME
// helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying
// run loiter controller
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);
// force descent rate and call position controller
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
pos_control.update_z_controller();
#else
wp_nav.init_loiter_target();
// multicopters do not stabilize roll/pitch/yaw when motors are stopped
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
#endif // HELI_FRAME
break;
case Loiter_Takeoff:
@ -150,14 +124,9 @@ void Sub::loiter_run()
case Loiter_Landed:
wp_nav.init_loiter_target();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
break;

32
ArduSub/control_rtl.cpp
View File

@ -141,14 +141,10 @@ void Sub::rtl_climb_return_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// To-Do: re-initialise wpnav targets
return;
}
@ -203,14 +199,10 @@ void Sub::rtl_loiterathome_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// To-Do: re-initialise wpnav targets
return;
}
@ -279,13 +271,9 @@ void Sub::rtl_descent_run()
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// set target to current position
wp_nav.init_loiter_target();
return;
@ -347,13 +335,9 @@ void Sub::rtl_land_run()
float target_yaw_rate = 0;
// if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// set target to current position
wp_nav.init_loiter_target();

6
ArduSub/esc_calibration.cpp
View File

@ -20,7 +20,6 @@ enum ESCCalibrationModes {
// check if we should enter esc calibration mode
void Sub::esc_calibration_startup_check()
{
#if FRAME_CONFIG != HELI_FRAME
// exit immediately if pre-arm rc checks fail
pre_arm_rc_checks();
if (!ap.pre_arm_rc_check) {
@ -71,13 +70,11 @@ void Sub::esc_calibration_startup_check()
if (g.esc_calibrate != ESCCAL_DISABLED) {
g.esc_calibrate.set_and_save(ESCCAL_NONE);
}
#endif // FRAME_CONFIG != HELI_FRAME
}
// esc_calibration_passthrough - pass through pilot throttle to escs
void Sub::esc_calibration_passthrough()
{
#if FRAME_CONFIG != HELI_FRAME
// clear esc flag for next time
g.esc_calibrate.set_and_save(ESCCAL_NONE);
@ -102,13 +99,11 @@ void Sub::esc_calibration_passthrough()
// pass through to motors
motors.throttle_pass_through(channel_throttle->radio_in);
}
#endif // FRAME_CONFIG != HELI_FRAME
}
// esc_calibration_auto - calibrate the ESCs automatically using a timer and no pilot input
void Sub::esc_calibration_auto()
{
#if FRAME_CONFIG != HELI_FRAME
bool printed_msg = false;
// reduce update rate to motors to 50Hz
@ -148,5 +143,4 @@ void Sub::esc_calibration_auto()
// block until we restart
while(1) { delay(5); }
#endif // FRAME_CONFIG != HELI_FRAME
}

46
ArduSub/flight_mode.cpp
View File

@ -24,19 +24,11 @@ bool Sub::set_mode(uint8_t mode)
switch(mode) {
case ACRO:
#if FRAME_CONFIG == HELI_FRAME
success = heli_acro_init(ignore_checks);
#else
success = acro_init(ignore_checks);
#endif
success = acro_init(ignore_checks);
break;
case STABILIZE:
#if FRAME_CONFIG == HELI_FRAME
success = heli_stabilize_init(ignore_checks);
#else
success = stabilize_init(ignore_checks);
#endif
success = stabilize_init(ignore_checks);
break;
case ALT_HOLD:
@ -136,19 +128,11 @@ void Sub::update_flight_mode()
switch (control_mode) {
case ACRO:
#if FRAME_CONFIG == HELI_FRAME
heli_acro_run();
#else
acro_run();
#endif
acro_run();
break;
case STABILIZE:
#if FRAME_CONFIG == HELI_FRAME
heli_stabilize_run();
#else
stabilize_run();
#endif
stabilize_run();
break;
case ALT_HOLD:
@ -236,28 +220,6 @@ void Sub::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode)
// cancel any takeoffs in progress
takeoff_stop();
#if FRAME_CONFIG == HELI_FRAME
// firmly reset the flybar passthrough to false when exiting acro mode.
if (old_control_mode == ACRO) {
attitude_control.use_flybar_passthrough(false, false);
motors.set_acro_tail(false);
}
// if we are changing from a mode that did not use manual throttle,
// stab col ramp value should be pre-loaded to the correct value to avoid a twitch
// heli_stab_col_ramp should really only be active switching between Stabilize and Acro modes
if (!mode_has_manual_throttle(old_control_mode)){
if (new_control_mode == STABILIZE){
input_manager.set_stab_col_ramp(1.0);
} else if (new_control_mode == ACRO){
input_manager.set_stab_col_ramp(0.0);
}
}
// reset RC Passthrough to motors
motors.reset_radio_passthrough();
#endif //HELI_FRAME
}
// returns true or false whether mode requires GPS

203
ArduSub/heli.cpp
View File

@ -1,203 +0,0 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Sub.h"
// Traditional helicopter variables and functions
#if FRAME_CONFIG == HELI_FRAME
#ifndef HELI_DYNAMIC_FLIGHT_SPEED_MIN
#define HELI_DYNAMIC_FLIGHT_SPEED_MIN 500 // we are in "dynamic flight" when the speed is over 1m/s for 2 seconds
#endif
// counter to control dynamic flight profile
static int8_t heli_dynamic_flight_counter;
// heli_init - perform any special initialisation required for the tradheli
void Copter::heli_init()
{
/*
automatically set H_RSC_MIN and H_RSC_MAX from RC8_MIN and
RC8_MAX so that when users upgrade from tradheli version 3.3 to
3.4 they get the same throttle range as in previous versions of
the code
*/
if (!g.heli_servo_rsc.radio_min.configured()) {
g.heli_servo_rsc.radio_min.set_and_save(g.rc_8.radio_min.get());
}
if (!g.heli_servo_rsc.radio_max.configured()) {
g.heli_servo_rsc.radio_max.set_and_save(g.rc_8.radio_max.get());
}
// pre-load stab col values as mode is initialized as Stabilize, but stabilize_init() function is not run on start-up.
input_manager.set_use_stab_col(true);
input_manager.set_stab_col_ramp(1.0);
}
// heli_check_dynamic_flight - updates the dynamic_flight flag based on our horizontal velocity
// should be called at 50hz
void Copter::check_dynamic_flight(void)
{
if (!motors.armed() || !motors.rotor_runup_complete() ||
control_mode == LAND || (control_mode==RTL && rtl_state == RTL_Land) || (control_mode == AUTO && auto_mode == Auto_Land)) {
heli_dynamic_flight_counter = 0;
heli_flags.dynamic_flight = false;
return;
}
bool moving = false;
// with GPS lock use inertial nav to determine if we are moving
if (position_ok()) {
// get horizontal velocity
float velocity = inertial_nav.get_velocity_xy();
moving = (velocity >= HELI_DYNAMIC_FLIGHT_SPEED_MIN);
}else{
// with no GPS lock base it on throttle and forward lean angle
moving = (motors.get_throttle() > 800.0f || ahrs.pitch_sensor < -1500);
}
if (!moving && sonar_enabled && sonar.status() == RangeFinder::RangeFinder_Good) {
// when we are more than 2m from the ground with good
// rangefinder lock consider it to be dynamic flight
moving = (sonar.distance_cm() > 200);
}
if (moving) {
// if moving for 2 seconds, set the dynamic flight flag
if (!heli_flags.dynamic_flight) {
heli_dynamic_flight_counter++;
if (heli_dynamic_flight_counter >= 100) {
heli_flags.dynamic_flight = true;
heli_dynamic_flight_counter = 100;
}
}
}else{
// if not moving for 2 seconds, clear the dynamic flight flag
if (heli_flags.dynamic_flight) {
if (heli_dynamic_flight_counter > 0) {
heli_dynamic_flight_counter--;
}else{
heli_flags.dynamic_flight = false;
}
}
}
}
// update_heli_control_dynamics - pushes several important factors up into AP_MotorsHeli.
// should be run between the rate controller and the servo updates.
void Copter::update_heli_control_dynamics(void)
{
static int16_t hover_roll_trim_scalar_slew = 0;
// Use Leaky_I if we are not moving fast
attitude_control.use_leaky_i(!heli_flags.dynamic_flight);
if (ap.land_complete || (motors.get_desired_rotor_speed() == 0)){
// if we are landed or there is no rotor power demanded, decrement slew scalar
hover_roll_trim_scalar_slew--;
} else {
// if we are not landed and motor power is demanded, increment slew scalar
hover_roll_trim_scalar_slew++;
}
hover_roll_trim_scalar_slew = constrain_int16(hover_roll_trim_scalar_slew, 0, MAIN_LOOP_RATE);
// set hover roll trim scalar, will ramp from 0 to 1 over 1 second after we think helicopter has taken off
attitude_control.set_hover_roll_trim_scalar((float)(hover_roll_trim_scalar_slew/MAIN_LOOP_RATE));
}
// heli_update_landing_swash - sets swash plate flag so higher minimum is used when landed or landing
// should be called soon after update_land_detector in main code
void Copter::heli_update_landing_swash()
{
switch(control_mode) {
case ACRO:
case STABILIZE:
case DRIFT:
case SPORT:
// manual modes always uses full swash range
motors.set_collective_for_landing(false);
break;
case LAND:
// landing always uses limit swash range
motors.set_collective_for_landing(true);
break;
case RTL:
if (rtl_state == RTL_Land) {
motors.set_collective_for_landing(true);
}else{
motors.set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed);
}
break;
case AUTO:
if (auto_mode == Auto_Land) {
motors.set_collective_for_landing(true);
}else{
motors.set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed);
}
break;
default:
// auto and hold use limited swash when landed
motors.set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed);
break;
}
}
// heli_update_rotor_speed_targets - reads pilot input and passes new rotor speed targets to heli motors object
void Copter::heli_update_rotor_speed_targets()
{
static bool rotor_runup_complete_last = false;
// get rotor control method
uint8_t rsc_control_mode = motors.get_rsc_mode();
rsc_control_deglitched = rotor_speed_deglitch_filter.apply(g.rc_8.control_in);
switch (rsc_control_mode) {
case ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH:
// pass through pilot desired rotor speed if control input is higher than 10, creating a deadband at the bottom
if (rsc_control_deglitched > 10) {
motors.set_interlock(true);
motors.set_desired_rotor_speed(rsc_control_deglitched);
} else {
motors.set_interlock(false);
motors.set_desired_rotor_speed(0);
}
break;
case ROTOR_CONTROL_MODE_SPEED_SETPOINT:
case ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT:
case ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT:
// pass setpoint through as desired rotor speed, this is almost pointless as the Setpoint serves no function in this mode
// other than being used to create a crude estimate of rotor speed
if (rsc_control_deglitched > 0) {
motors.set_interlock(true);
motors.set_desired_rotor_speed(motors.get_rsc_setpoint());
}else{
motors.set_interlock(false);
motors.set_desired_rotor_speed(0);
}
break;
}
// when rotor_runup_complete changes to true, log event
if (!rotor_runup_complete_last && motors.rotor_runup_complete()){
Log_Write_Event(DATA_ROTOR_RUNUP_COMPLETE);
} else if (rotor_runup_complete_last && !motors.rotor_runup_complete()){
Log_Write_Event(DATA_ROTOR_SPEED_BELOW_CRITICAL);
}
rotor_runup_complete_last = motors.rotor_runup_complete();
}
// heli_radio_passthrough send RC inputs direct into motors library for use during manual passthrough for helicopter setup
void Copter::heli_radio_passthrough()
{
motors.set_radio_passthrough(channel_roll->control_in, channel_pitch->control_in, channel_throttle->control_in, channel_yaw->control_in);
}
#endif // FRAME_CONFIG == HELI_FRAME

98
ArduSub/heli_control_acro.cpp
View File

@ -1,98 +0,0 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Sub.h"
#if FRAME_CONFIG == HELI_FRAME
/*
* heli_control_acro.pde - init and run calls for acro flight mode for trad heli
*/
// heli_acro_init - initialise acro controller
bool Copter::heli_acro_init(bool ignore_checks)
{
// if heli is equipped with a flybar, then tell the attitude controller to pass through controls directly to servos
attitude_control.use_flybar_passthrough(motors.has_flybar(), motors.supports_yaw_passthrough());
motors.set_acro_tail(true);
// set stab collective false to use full collective pitch range
input_manager.set_use_stab_col(false);
// always successfully enter acro
return true;
}
// heli_acro_run - runs the acro controller
// should be called at 100hz or more
void Copter::heli_acro_run()
{
float target_roll, target_pitch, target_yaw;
int16_t pilot_throttle_scaled;
// Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because
// we may be in autorotation flight. These should be reset only when transitioning from disarmed
// to armed, because the pilot will have placed the helicopter down on the landing pad. This is so
// that the servos move in a realistic fashion while disarmed for operational checks.
// Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
if(!motors.armed()) {
heli_flags.init_targets_on_arming=true;
attitude_control.set_yaw_target_to_current_heading();
}
if(motors.armed() && heli_flags.init_targets_on_arming) {
attitude_control.set_yaw_target_to_current_heading();
if (motors.rotor_speed_above_critical()) {
heli_flags.init_targets_on_arming=false;
}
}
// send RC inputs direct into motors library for use during manual passthrough for helicopter setup
heli_radio_passthrough();
if (!motors.has_flybar()){
// convert the input to the desired body frame rate
get_pilot_desired_angle_rates(channel_roll->control_in, channel_pitch->control_in, channel_yaw->control_in, target_roll, target_pitch, target_yaw);
if (motors.supports_yaw_passthrough()) {
// if the tail on a flybar heli has an external gyro then
// also use no deadzone for the yaw control and
// pass-through the input direct to output.
target_yaw = channel_yaw->pwm_to_angle_dz(0);
}
// run attitude controller
attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
}else{
/*
for fly-bar passthrough use control_in values with no
deadzone. This gives true pass-through.
*/
float roll_in = channel_roll->pwm_to_angle_dz(0);
float pitch_in = channel_pitch->pwm_to_angle_dz(0);
float yaw_in;
if (motors.supports_yaw_passthrough()) {
// if the tail on a flybar heli has an external gyro then
// also use no deadzone for the yaw control and
// pass-through the input direct to output.
yaw_in = channel_yaw->pwm_to_angle_dz(0);
} else {
// if there is no external gyro then run the usual
// ACRO_YAW_P gain on the input control, including
// deadzone
yaw_in = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// run attitude controller
attitude_control.passthrough_bf_roll_pitch_rate_yaw(roll_in, pitch_in, yaw_in);
}
// get pilot's desired throttle
pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->control_in);
// output pilot's throttle without angle boost
attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
}
#endif //HELI_FRAME

72
ArduSub/heli_control_stabilize.cpp
View File

@ -1,72 +0,0 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Sub.h"
#if FRAME_CONFIG == HELI_FRAME
/*
* heli_control_stabilize.pde - init and run calls for stabilize flight mode for trad heli
*/
// stabilize_init - initialise stabilize controller
bool Copter::heli_stabilize_init(bool ignore_checks)
{
// set target altitude to zero for reporting
// To-Do: make pos controller aware when it's active/inactive so it can always report the altitude error?
pos_control.set_alt_target(0);
// set stab collective true to use stabilize scaled collective pitch range
input_manager.set_use_stab_col(true);
return true;
}
// stabilize_run - runs the main stabilize controller
// should be called at 100hz or more
void Copter::heli_stabilize_run()
{
float target_roll, target_pitch;
float target_yaw_rate;
int16_t pilot_throttle_scaled;
// Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because
// we may be in autorotation flight. These should be reset only when transitioning from disarmed
// to armed, because the pilot will have placed the helicopter down on the landing pad. This is so
// that the servos move in a realistic fashion while disarmed for operational checks.
// Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
if(!motors.armed()) {
heli_flags.init_targets_on_arming=true;
attitude_control.set_yaw_target_to_current_heading();
}
if(motors.armed() && heli_flags.init_targets_on_arming) {
attitude_control.set_yaw_target_to_current_heading();
if (motors.rotor_speed_above_critical()) {
heli_flags.init_targets_on_arming=false;
}
}
// send RC inputs direct into motors library for use during manual passthrough for helicopter setup
heli_radio_passthrough();
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
// get pilot's desired throttle
pilot_throttle_scaled = input_manager.get_pilot_desired_collective(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());
// output pilot's throttle - note that TradHeli does not used angle-boost
attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
}
#endif //HELI_FRAME

2
ArduSub/land_detector.cpp
View File

@ -52,7 +52,6 @@ void Sub::set_land_complete(bool b)
// has no effect when throttle is above hover throttle
void Sub::update_throttle_thr_mix()
{
#if FRAME_CONFIG != HELI_FRAME
// if disarmed or landed prioritise throttle
if(!motors.armed() || ap.land_complete) {
motors.set_throttle_mix_min();
@ -91,5 +90,4 @@ void Sub::update_throttle_thr_mix()
motors.set_throttle_mix_min();
}
}
#endif
}

10
ArduSub/motors.cpp
View File

@ -99,21 +99,11 @@ void Sub::auto_disarm_check()
return;
}
#if FRAME_CONFIG == HELI_FRAME
// if the rotor is still spinning, don't initiate auto disarm
if (motors.rotor_speed_above_critical()) {
auto_disarm_begin = tnow_ms;
return;
}
#endif
// always allow auto disarm if using interlock switch or motors are Emergency Stopped
if ((ap.using_interlock && !motors.get_interlock()) || ap.motor_emergency_stop) {
#if FRAME_CONFIG != HELI_FRAME
// use a shorter delay if using throttle interlock switch or Emergency Stop, because it is less
// obvious the copter is armed as the motors will not be spinning
disarm_delay_ms /= 2;
#endif
} else {
bool sprung_throttle_stick = (g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0;
bool thr_low;

10
ArduSub/radio.cpp
View File

@ -10,14 +10,8 @@ void Sub::default_dead_zones()
{
channel_roll->set_default_dead_zone(30);
channel_pitch->set_default_dead_zone(30);
#if FRAME_CONFIG == HELI_FRAME
channel_throttle->set_default_dead_zone(10);
channel_yaw->set_default_dead_zone(15);
g.rc_8.set_default_dead_zone(10);
#else
channel_throttle->set_default_dead_zone(30);
channel_yaw->set_default_dead_zone(40);
#endif
g.rc_6.set_default_dead_zone(0);
}
@ -72,10 +66,6 @@ void Sub::init_rc_out()
motors.set_update_rate(g.rc_speed);
motors.set_frame_orientation(g.frame_orientation);
motors.Init(); // motor initialisation
#if FRAME_CONFIG != HELI_FRAME
motors.set_throttle_range(g.throttle_min, channel_throttle->radio_min, channel_throttle->radio_max);
motors.set_hover_throttle(g.throttle_mid);
#endif
for(uint8_t i = 0; i < 5; i++) {
delay(20);

20
ArduSub/system.cpp
View File

@ -164,11 +164,6 @@ void Sub::init_ardupilot()
// update motor interlock state
update_using_interlock();
#if FRAME_CONFIG == HELI_FRAME
// trad heli specific initialisation
heli_init();
#endif
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up motors and output to escs
@ -397,27 +392,12 @@ void Sub::update_auto_armed()
if(mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.radio) {
set_auto_armed(false);
}
#if FRAME_CONFIG == HELI_FRAME
// if helicopters are on the ground, and the motor is switched off, auto-armed should be false
// so that rotor runup is checked again before attempting to take-off
if(ap.land_complete && !motors.rotor_runup_complete()) {
set_auto_armed(false);
}
#endif // HELI_FRAME
}else{
// arm checks
#if FRAME_CONFIG == HELI_FRAME
// for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true
if(motors.armed() && !ap.throttle_zero && motors.rotor_runup_complete()) {
set_auto_armed(true);
}
#else
// if motors are armed and throttle is above zero auto_armed should be true
if(motors.armed() && !ap.throttle_zero) {
set_auto_armed(true);
}
#endif // HELI_FRAME
}
}

7
ArduSub/takeoff.cpp
View File

@ -29,13 +29,6 @@ bool Sub::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
{
if (motors.armed() && ap.land_complete && current_mode_has_user_takeoff(must_navigate) && takeoff_alt_cm > current_loc.alt) {
#if FRAME_CONFIG == HELI_FRAME
// Helicopters should return false if MAVlink takeoff command is received while the rotor is not spinning
if (!motors.rotor_runup_complete()) {
return false;
}
#endif
switch(control_mode) {
case GUIDED:
set_auto_armed(true);

20
ArduSub/tuning.cpp
View File

@ -106,24 +106,6 @@ void Sub::tuning() {
g.acro_yaw_p = tuning_value;
break;
#if FRAME_CONFIG == HELI_FRAME
case TUNING_HELI_EXTERNAL_GYRO:
motors.ext_gyro_gain(g.rc_6.control_in);
break;
case TUNING_RATE_PITCH_FF:
g.pid_rate_pitch.ff(tuning_value);
break;
case TUNING_RATE_ROLL_FF:
g.pid_rate_roll.ff(tuning_value);
break;
case TUNING_RATE_YAW_FF:
g.pid_rate_yaw.ff(tuning_value);
break;
#endif
case TUNING_DECLINATION:
// set declination to +-20degrees
compass.set_declination(ToRad((2.0f * g.rc_6.control_in - g.radio_tuning_high)/100.0f), false); // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact
@ -199,11 +181,9 @@ void Sub::tuning() {
g.pid_rate_roll.kD(tuning_value);
break;
#if FRAME_CONFIG != HELI_FRAME
case TUNING_RATE_MOT_YAW_HEADROOM:
motors.set_yaw_headroom(tuning_value*1000);
break;
#endif
case TUNING_RATE_YAW_FILT:
g.pid_rate_yaw.filt_hz(tuning_value);