3 #if MODE_THROW_ENABLED == ENABLED 8 #if FRAME_CONFIG == HELI_FRAME 47 gcs().
send_text(MAV_SEVERITY_INFO,
"throw detected - uprighting");
54 gcs().
send_text(MAV_SEVERITY_INFO,
"uprighted - controlling height");
75 copter.set_auto_armed(
true);
78 gcs().
send_text(MAV_SEVERITY_INFO,
"height achieved - controlling position");
85 copter.set_auto_armed(
true);
190 const float accel =
copter.ins.get_accel().length();
198 "TimeUS,Stage,Vel,VelZ,Acc,AccEfZ,Throw,AttOk,HgtOk,PosOk",
227 bool changing_height;
241 bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action;
253 if (throw_condition_confirmed) {
264 return (rotMat.
c.
z > 0.866f);
bool set_mode(control_mode_t mode, mode_reason_t reason)
virtual float get_velocity_z() const=0
uint32_t waiting_for_throw
float get_alt_error() const
virtual nav_filter_status get_filter_status() const=0
void update_z_controller()
float get_horizontal_error() const
#define THROW_VERTICAL_SPEED
AC_AttitudeControl_t *& attitude_control
void init_target(const Vector3f &position)
virtual const Matrix3f & get_rotation_body_to_ned(void) const=0
ThrowModeStage prev_stage
void update(float ekfGndSpdLimit, float ekfNavVelGainScaler)
uint32_t free_fall_start_ms
void Log_Write(const char *name, const char *labels, const char *fmt,...)
virtual const Vector3f & get_accel_ef(uint8_t i) const
void set_desired_velocity_z(float vel_z_cms)
void set_speed_z(float speed_down, float speed_up)
static DataFlash_Class * instance(void)
#define BRAKE_MODE_SPEED_Z
#define BRAKE_MODE_DECEL_RATE
void set_accel_z(float accel_cmss)
void send_text(MAV_SEVERITY severity, const char *fmt,...)
DESIRED_THROTTLE_UNLIMITED
virtual void set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend)
AC_PosControl *& pos_control
bool throw_position_good()
struct nav_filter_status::@138 flags
AP_Int8 throw_motor_start
void set_alt_target(float alt_cm)
static struct notify_flags_and_values_type flags
AP_HAL_MAIN_CALLBACKS & copter
bool throw_attitude_good()
float & ekfNavVelGainScaler
AP_InertialNav & inertial_nav
virtual const Vector3f & get_velocity() const=0
virtual float get_altitude() const=0
int32_t get_pitch() const
float free_fall_start_velz
bool init(bool ignore_checks) override