mirror of https://github.com/ArduPilot/ardupilot
TradHeli: Incorporating Ext ESC Control
This commit is contained in:
Robert Lefebvre
parent
30e55533dc
commit
5f058fb9b2
|
|
@ -403,9 +403,9 @@ static byte oldSwitchPosition;
|
|||
|
||||
#if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments
|
||||
#if INSTANT_PWM == 1
|
||||
MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4, AP_MOTORS_SPEED_INSTANT_PWM); // this hardware definition is slightly bad because it assumes APM_HARDWARE_APM2 == AP_MOTORS_APM2
|
||||
MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4, AP_MOTORS_SPEED_INSTANT_PWM); // this hardware definition is slightly bad because it assumes APM_HARDWARE_APM2 == AP_MOTORS_APM2
|
||||
#else
|
||||
MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4);
|
||||
MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4);
|
||||
#endif
|
||||
#elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing
|
||||
#if INSTANT_PWM == 1
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@
|
|||
#include "AP_MotorsHeli.h"
|
||||
|
||||
const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
|
||||
AP_NESTEDGROUPINFO(AP_Motors, 0),
|
||||
|
||||
|
||||
// @Param: SV1_POS
|
||||
// @DisplayName: Servo 1 Position
|
||||
|
|
@ -96,7 +96,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
|
|||
// @DisplayName: Swash Plate Type
|
||||
// @Description: Setting this to 0 will configure for a 3-servo CCPM. Setting this to 1 will configure for mechanically mixed "H1".
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("SWASH_TYPE", 10, AP_MotorsHeli, swash_type), // changed from trunk
|
||||
AP_GROUPINFO("SWASH_TYPE", 10, AP_MotorsHeli, swash_type),
|
||||
|
||||
// @Param: GYR_GAIM
|
||||
// @DisplayName: External Gyro Gain
|
||||
|
|
@ -121,13 +121,23 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
|
|||
// @Units: Degrees
|
||||
// @User: Advanced
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("PHANG", 13, AP_MotorsHeli, phase_angle), // changed from trunk
|
||||
AP_GROUPINFO("PHANG", 13, AP_MotorsHeli, phase_angle),
|
||||
|
||||
// @Param: COLYAW
|
||||
// @DisplayName: Collective-Yaw Mixing
|
||||
// @Description: This is a feed-forward compensation to automatically add rudder input when collective pitch is increased.
|
||||
// @Range: 0 5
|
||||
AP_GROUPINFO("COLYAW", 14, AP_MotorsHeli, collective_yaw_effect), // changed from trunk
|
||||
AP_GROUPINFO("COLYAW", 14, AP_MotorsHeli, collective_yaw_effect),
|
||||
|
||||
// @Param: GOV_SETPOINT
|
||||
// @DisplayName: External Motor Governor Setpoint
|
||||
// @Description: This is the PWM which is passed to the external motor governor when external governor is enabled.
|
||||
// @Range: 1000 2000
|
||||
// @Units: PWM
|
||||
// @Increment: 10
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("GOV_SETPOINT", 15, AP_MotorsHeli, ext_gov_setpoint),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
|
@ -159,6 +169,7 @@ void AP_MotorsHeli::enable()
|
|||
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_3]); // swash servo 3
|
||||
_rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_4]); // yaw
|
||||
_rc->enable_out(AP_MOTORS_HELI_EXT_GYRO); // for external gyro
|
||||
_rc->enable_out(AP_MOTORS_HELI_EXT_ESC); // for external ESC
|
||||
}
|
||||
|
||||
// output_min - sends minimum values out to the motors
|
||||
|
|
@ -186,6 +197,8 @@ void AP_MotorsHeli::output_armed()
|
|||
_rc_yaw->calc_pwm();
|
||||
|
||||
move_swash( _rc_roll->servo_out, _rc_pitch->servo_out, _rc_throttle->servo_out, _rc_yaw->servo_out );
|
||||
|
||||
ext_esc_control();
|
||||
}
|
||||
|
||||
// output_disarmed - sends commands to the motors
|
||||
|
|
@ -470,3 +483,61 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll
|
|||
_rc->Force_Out2_Out3();
|
||||
}
|
||||
}
|
||||
|
||||
void AP_MotorsHeli::ext_esc_control()
|
||||
|
||||
{
|
||||
switch ( AP_MOTORS_ESC_MODE_PASSTHROUGH ) {
|
||||
|
||||
case AP_MOTORS_ESC_MODE_PASSTHROUGH:
|
||||
if( armed() ){
|
||||
_rc->OutputCh(AP_MOTORS_HELI_EXT_ESC, _rc_8->radio_in);
|
||||
} else {
|
||||
_rc->OutputCh(AP_MOTORS_HELI_EXT_ESC, _rc_8->radio_min);
|
||||
}
|
||||
break;
|
||||
|
||||
case AP_MOTORS_ESC_MODE_EXT_GOV:
|
||||
|
||||
if( armed() && _rc_throttle->control_in > 10){
|
||||
if (ext_esc_ramp < AP_MOTORS_EXT_ESC_RAMP_UP){
|
||||
ext_esc_ramp++;
|
||||
ext_esc_output = map(ext_esc_ramp, 0, AP_MOTORS_EXT_ESC_RAMP_UP, 1000, ext_gov_setpoint);
|
||||
} else {
|
||||
ext_esc_output = ext_gov_setpoint;
|
||||
}
|
||||
} else {
|
||||
ext_esc_ramp = 0; //Return ESC Ramp to 0
|
||||
ext_esc_output = 1000; //Just to be sure ESC output is 0
|
||||
}
|
||||
_rc->OutputCh(AP_MOTORS_HELI_EXT_ESC, ext_esc_output);
|
||||
break;
|
||||
|
||||
// case 3: // Open Loop ESC Control
|
||||
//
|
||||
// coll_scaled = _motors->coll_out_scaled + 1000;
|
||||
// if(coll_scaled <= _motors->collective_mid){
|
||||
// esc_ol_output = map(coll_scaled, _motors->collective_min, _motors->collective_mid, esc_out_low, esc_out_mid); // Bottom half of V-curve
|
||||
// } else if (coll_scaled > _motors->collective_mid){
|
||||
// esc_ol_output = map(coll_scaled, _motors->collective_mid, _motors->collective_max, esc_out_mid, esc_out_high); // Top half of V-curve
|
||||
// } else { esc_ol_output = 1000; } // Just in case.
|
||||
//
|
||||
// if(_motors->armed() && _rc_throttle->control_in > 10){
|
||||
// if (ext_esc_ramp < ext_esc_ramp_up){
|
||||
// ext_esc_ramp++;
|
||||
// ext_esc_output = map(ext_esc_ramp, 0, ext_esc_ramp_up, 1000, esc_ol_output);
|
||||
// } else {
|
||||
// ext_esc_output = esc_ol_output;
|
||||
// }
|
||||
// } else {
|
||||
// ext_esc_ramp = 0; //Return ESC Ramp to 0
|
||||
// ext_esc_output = 1000; //Just to be sure ESC output is 0
|
||||
//}
|
||||
// _rc->OutputCh(AP_MOTORS_HELI_EXT_ESC, ext_esc_output);
|
||||
// break;
|
||||
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
};
|
||||
|
|
@ -27,6 +27,17 @@
|
|||
#define AP_MOTORS_HELI_SWASH_CCPM 0
|
||||
#define AP_MOTORS_HELI_SWASH_H1 1
|
||||
|
||||
// ext ESC definitions
|
||||
#define AP_MOTORS_HELI_EXT_ESC CH_8
|
||||
#define AP_MOTORS_ESC_MODE_PASSTHROUGH 1
|
||||
#define AP_MOTORS_ESC_MODE_EXT_GOV 2
|
||||
#define AP_MOTORS_EXT_ESC_RAMP_UP 1000 //Temporary until we can make this a parameter
|
||||
#define AP_MOTORS_EXT_ESC ACTIVE AP_MOTORS_ESC_MODE_PASSTHROUGH //Temporary until we can make this a parameter
|
||||
|
||||
|
||||
|
||||
class AP_HeliControls;
|
||||
|
||||
/// @class AP_MotorsHeli
|
||||
class AP_MotorsHeli : public AP_Motors {
|
||||
public:
|
||||
|
|
@ -38,6 +49,7 @@ public:
|
|||
RC_Channel* rc_pitch,
|
||||
RC_Channel* rc_throttle,
|
||||
RC_Channel* rc_yaw,
|
||||
RC_Channel* rc_8,
|
||||
RC_Channel* swash_servo_1,
|
||||
RC_Channel* swash_servo_2,
|
||||
RC_Channel* swash_servo_3,
|
||||
|
|
@ -48,6 +60,7 @@ public:
|
|||
_servo_2(swash_servo_2),
|
||||
_servo_3(swash_servo_3),
|
||||
_servo_4(yaw_servo),
|
||||
_rc_8(rc_8),
|
||||
swash_type(AP_MOTORS_HELI_SWASH_CCPM),
|
||||
servo1_pos (-60),
|
||||
servo2_pos (60),
|
||||
|
|
@ -63,77 +76,92 @@ public:
|
|||
collective_yaw_effect (0),
|
||||
servo_manual (0),
|
||||
throttle_mid(0),
|
||||
ext_gov_setpoint(1500),
|
||||
_roll_scaler(1),
|
||||
_pitch_scaler(1),
|
||||
_collective_scalar(1),
|
||||
_swash_initialised(false)
|
||||
{};
|
||||
|
||||
// init
|
||||
virtual void Init();
|
||||
|
||||
// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
|
||||
// you must have setup_motors before calling this
|
||||
virtual void set_update_rate( uint16_t speed_hz );
|
||||
|
||||
// enable - starts allowing signals to be sent to motors
|
||||
virtual void enable();
|
||||
|
||||
// get basic information about the platform
|
||||
virtual uint8_t get_num_motors() { return 4; };
|
||||
|
||||
// motor test
|
||||
virtual void output_test();
|
||||
|
||||
// output_min - sends minimum values out to the motors
|
||||
virtual void output_min();
|
||||
|
||||
// reset_swash - free up swash for maximum movements. Used for set-up
|
||||
virtual void reset_swash();
|
||||
|
||||
// init_swash - initialise the swash plate
|
||||
virtual void init_swash();
|
||||
|
||||
// heli_move_swash - moves swash plate to attitude of parameters passed in
|
||||
virtual void move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_out, int16_t yaw_out);
|
||||
|
||||
// var_info for holding Parameter information
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
//protected:
|
||||
// output - sends commands to the motors
|
||||
virtual void output_armed();
|
||||
virtual void output_disarmed();
|
||||
|
||||
float _rollFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
|
||||
float _pitchFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
|
||||
float _collectiveFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
|
||||
|
||||
RC_Channel *_servo_1;
|
||||
RC_Channel *_servo_2;
|
||||
RC_Channel *_servo_3;
|
||||
RC_Channel *_servo_4;
|
||||
RC_Channel *_rc_8;
|
||||
AP_Int8 swash_type;
|
||||
AP_Int16 servo1_pos;
|
||||
AP_Int16 servo2_pos;
|
||||
AP_Int16 servo3_pos;
|
||||
AP_Int16 roll_max;
|
||||
AP_Int16 pitch_max;
|
||||
AP_Int16 collective_min;
|
||||
AP_Int16 collective_max;
|
||||
AP_Int16 collective_mid;
|
||||
AP_Int16 ext_gyro_enabled;
|
||||
AP_Int16 ext_gyro_gain;
|
||||
AP_Int16 roll_max;
|
||||
AP_Int16 pitch_max;
|
||||
AP_Int16 phase_angle;
|
||||
AP_Int16 collective_yaw_effect;
|
||||
AP_Int8 servo_manual; // used to trigger swash reset from mission planner
|
||||
AP_Int16 ext_gov_setpoint; // maximum output to the motor governor
|
||||
int16_t throttle_mid; // throttle mid point in pwm form (i.e. 0 ~ 1000)
|
||||
int16_t coll_out_scaled;
|
||||
|
||||
|
||||
// init
|
||||
void Init();
|
||||
|
||||
// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm
|
||||
// you must have setup_motors before calling this
|
||||
void set_update_rate( uint16_t speed_hz );
|
||||
|
||||
// enable - starts allowing signals to be sent to motors
|
||||
void enable();
|
||||
|
||||
// get basic information about the platform
|
||||
uint8_t get_num_motors() { return 5; };
|
||||
|
||||
// motor test
|
||||
void output_test();
|
||||
|
||||
// output_min - sends minimum values out to the motors
|
||||
void output_min();
|
||||
|
||||
// init_swash - initialise the swash plate
|
||||
void init_swash();
|
||||
|
||||
// output - sends commands to the motors
|
||||
void output_armed();
|
||||
|
||||
// var_info for holding Parameter information
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
protected:
|
||||
|
||||
// heli_move_swash - moves swash plate to attitude of parameters passed in
|
||||
void move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_out, int16_t yaw_out);
|
||||
|
||||
// reset_swash - free up swash for maximum movements. Used for set-up
|
||||
void reset_swash();
|
||||
|
||||
void output_disarmed();
|
||||
|
||||
void ext_esc_control();
|
||||
|
||||
float _rollFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
|
||||
float _pitchFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
|
||||
float _collectiveFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];
|
||||
|
||||
|
||||
|
||||
// internally used variables
|
||||
int16_t throttle_mid; // throttle mid point in pwm form (i.e. 0 ~ 1000)
|
||||
|
||||
float _roll_scaler; // scaler to convert roll input from radio (i.e. -4500 ~ 4500) to max roll range
|
||||
float _pitch_scaler; // scaler to convert pitch input from radio (i.e. -4500 ~ 4500) to max pitch range
|
||||
float _collective_scalar; // throttle scalar to convert pwm form (i.e. 0 ~ 1000) passed in to actual servo range (i.e 1250~1750 would be 500)
|
||||
bool _swash_initialised; // true if swash has been initialised
|
||||
int16_t ext_esc_output; // final output to the external motor governor 1000-2000
|
||||
int16_t ext_esc_ramp; // current state of ramping
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue