123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197 |
- #pragma once
- #include "AP_Motors_Class.h"
- #ifndef AP_MOTORS_DENSITY_COMP
- #define AP_MOTORS_DENSITY_COMP 1
- #endif
- #define AP_MOTORS_YAW_HEADROOM_DEFAULT 200
- #define AP_MOTORS_THST_EXPO_DEFAULT 0.65f
- #define AP_MOTORS_THST_HOVER_DEFAULT 0.35f
- #define AP_MOTORS_THST_HOVER_TC 10.0f
- #define AP_MOTORS_THST_HOVER_MIN 0.125f
- #define AP_MOTORS_THST_HOVER_MAX 0.6875f
- #define AP_MOTORS_SPIN_MIN_DEFAULT 0.15f
- #define AP_MOTORS_SPIN_MAX_DEFAULT 0.95f
- #define AP_MOTORS_SPIN_ARM_DEFAULT 0.10f
- #define AP_MOTORS_BAT_VOLT_MAX_DEFAULT 0.0f
- #define AP_MOTORS_BAT_VOLT_MIN_DEFAULT 0.0f
- #define AP_MOTORS_BAT_CURR_MAX_DEFAULT 0.0f
- #define AP_MOTORS_BAT_CURR_TC_DEFAULT 5.0f
- #define AP_MOTORS_BATT_VOLT_FILT_HZ 0.5f
- #define AP_MOTORS_SLEW_TIME_DEFAULT 0.0f
- #define AP_MOTORS_SPOOL_UP_TIME_DEFAULT 0.5f
- class AP_MotorsMulticopter : public AP_Motors {
- public:
-
- AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);
-
- virtual void output() override;
-
- void output_min() override;
-
- void set_yaw_headroom(int16_t pwm) { _yaw_headroom = pwm; }
-
-
- void set_throttle_range(int16_t radio_min, int16_t radio_max);
-
- void update_throttle_hover(float dt);
- virtual float get_throttle_hover() const override { return _throttle_hover; }
-
-
- void set_throttle_passthrough_for_esc_calibration(float throttle_input);
-
- float get_lift_max() { return _lift_max; }
-
- float get_batt_voltage_filt() const { return _batt_voltage_filt.get(); }
-
- float get_throttle_limit() const { return _throttle_limit; }
-
- float get_throttle_thrust_max() const { return _throttle_thrust_max; }
-
- bool spool_up_complete() const { return _spool_state == SpoolState::THROTTLE_UNLIMITED; }
-
-
-
- virtual void output_motor_mask(float thrust, uint8_t mask, float rudder_dt);
-
-
- virtual uint16_t get_motor_mask() override;
-
- int16_t get_pwm_output_min() const;
- int16_t get_pwm_output_max() const;
-
-
- FUNCTOR_TYPEDEF(thrust_compensation_fn_t, void, float *, uint8_t);
- void set_thrust_compensation_callback(thrust_compensation_fn_t callback) {
- _thrust_compensation_callback = callback;
- }
-
-
- static const struct AP_Param::GroupInfo var_info[];
- bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS];
- protected:
-
- void output_logic();
-
- virtual void output_to_motors() = 0;
- virtual void output_to_PWM() = 0;
-
- virtual void update_throttle_filter() override;
-
- virtual float get_current_limit_max_throttle();
-
- float apply_thrust_curve_and_volt_scaling(float thrust) const;
-
- void update_lift_max_from_batt_voltage();
-
- float get_compensation_gain() const;
-
- int16_t output_to_pwm(float _actuator_output);
-
- float thrust_to_actuator(float thrust_in);
-
- void set_actuator_with_slew(float& actuator_output, float input);
-
- float actuator_spin_up_to_ground_idle() const;
-
- virtual void thrust_compensation(void) {}
-
- virtual void output_boost_throttle(void);
-
-
- void save_params_on_disarm() override;
-
- enum HoverLearn {
- HOVER_LEARN_DISABLED = 0,
- HOVER_LEARN_ONLY = 1,
- HOVER_LEARN_AND_SAVE = 2
- };
-
- AP_Int16 _yaw_headroom;
- AP_Float _thrust_curve_expo;
- AP_Float _slew_up_time;
- AP_Float _slew_dn_time;
- AP_Float _spin_min;
- AP_Float _spin_max;
- AP_Float _spin_arm;
- AP_Float _batt_voltage_max;
- AP_Float _batt_voltage_min;
- AP_Float _batt_current_max;
- AP_Float _batt_current_time_constant;
- AP_Int8 _batt_idx;
- AP_Int16 _pwm_min;
- AP_Int16 _pwm_max;
- AP_Float _throttle_hover;
- AP_Int8 _throttle_hover_learn;
- AP_Int8 _disarm_disable_pwm;
-
- AP_Float _yaw_servo_angle_max_deg;
-
- AP_Float _spool_up_time;
-
- AP_Float _boost_scale;
-
-
- int16_t _throttle_radio_min;
- int16_t _throttle_radio_max;
-
-
- float _spin_up_ratio;
-
- LowPassFilterFloat _batt_voltage_filt;
- float _lift_max;
- float _throttle_limit;
- float _throttle_thrust_max;
- uint16_t _disarm_safety_timer;
-
- thrust_compensation_fn_t _thrust_compensation_callback;
-
- float _actuator[AP_MOTORS_MAX_NUM_MOTORS];
- };
|