mirror of https://github.com/ArduPilot/ardupilot
RC_Channel: Allow each channel to know who they are, this simplifies the enabling and output function calls.
This commit is contained in:
parent
afd96025a7
commit
b8c7b8a786
|
@ -1271,10 +1271,6 @@ static void fifty_hz_loop()
|
|||
// set-up channel 6 to control pitch
|
||||
camera_mount.set_manual_rc_channel( &g.rc_6 );
|
||||
|
||||
// enable output channels
|
||||
g.rc_camera_roll.enable_out_ch(CH_CAM_ROLL);
|
||||
g.rc_camera_pitch.enable_out_ch(CH_CAM_PITCH);
|
||||
g.rc_camera_yaw.enable_out_ch(CH_CAM_YAW);
|
||||
}
|
||||
|
||||
// update pilot's commands to mount
|
||||
|
@ -1288,9 +1284,9 @@ static void fifty_hz_loop()
|
|||
camera_mount.update_mount_position();
|
||||
|
||||
// move camera servos. TO-DO: move this to AP_Mount library
|
||||
g.rc_camera_roll.output_ch(CH_CAM_ROLL);
|
||||
g.rc_camera_pitch.output_ch(CH_CAM_PITCH);
|
||||
g.rc_camera_yaw.output_ch(CH_CAM_PITCH);
|
||||
g.rc_camera_roll.output();
|
||||
g.rc_camera_pitch.output();
|
||||
g.rc_camera_yaw.output();
|
||||
#endif
|
||||
|
||||
#if CAMERA == ENABLED
|
||||
|
@ -1354,6 +1350,7 @@ static void slow_loop()
|
|||
#if MOUNT == ENABLED
|
||||
update_aux_servo_function(&g.rc_camera_roll, &g.rc_camera_pitch, &g.rc_camera_yaw);
|
||||
#endif
|
||||
enable_aux_servos();
|
||||
|
||||
#if MOUNT == ENABLED
|
||||
camera_mount.update_mount_type();
|
||||
|
|
|
@ -399,6 +399,20 @@ public:
|
|||
ch7_option (CH7_OPTION),
|
||||
auto_slew_rate (AUTO_SLEW_RATE),
|
||||
|
||||
rc_1 (CH_1),
|
||||
rc_2 (CH_2),
|
||||
rc_3 (CH_3),
|
||||
rc_4 (CH_4),
|
||||
rc_5 (CH_5),
|
||||
rc_6 (CH_6),
|
||||
rc_7 (CH_7),
|
||||
rc_8 (CH_8),
|
||||
#if MOUNT == ENABLED
|
||||
rc_camera_roll (CH_9),
|
||||
rc_camera_pitch (CH_10),
|
||||
rc_camera_yaw (CH_11),
|
||||
#endif
|
||||
|
||||
rc_speed(RC_FAST_SPEED),
|
||||
|
||||
stabilize_d (STABILIZE_D),
|
||||
|
|
|
@ -64,14 +64,6 @@ static void init_rc_out()
|
|||
{
|
||||
APM_RC.Init( &isr_registry ); // APM Radio initialization
|
||||
|
||||
#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM1
|
||||
APM_RC.enable_out(CH_9);
|
||||
APM_RC.enable_out(CH_10);
|
||||
APM_RC.enable_out(CH_11);
|
||||
APM_RC.OutputCh(CH_9, g.rc_9.radio_trim);
|
||||
APM_RC.OutputCh(CH_10, g.rc_10.radio_trim);
|
||||
APM_RC.OutputCh(CH_11, g.rc_11.radio_trim);
|
||||
#endif
|
||||
#if INSTANT_PWM == 1
|
||||
motors.set_update_rate(AP_MOTORS_SPEED_INSTANT_PWM);
|
||||
#else
|
||||
|
@ -123,6 +115,8 @@ static void init_rc_out()
|
|||
// send miinimum throttle out to ESC
|
||||
output_min();
|
||||
}
|
||||
|
||||
enable_aux_servos();
|
||||
}
|
||||
|
||||
void output_min()
|
||||
|
|
|
@ -908,6 +908,7 @@ static void slow_loop()
|
|||
#else
|
||||
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_9, &g.rc_10, &g.rc_11);
|
||||
#endif
|
||||
enable_aux_servos();
|
||||
|
||||
#if MOUNT == ENABLED
|
||||
camera_mount.update_mount_type();
|
||||
|
|
|
@ -485,6 +485,20 @@ public:
|
|||
inverted_flight_ch (0),
|
||||
sonar_enabled (SONAR_ENABLED),
|
||||
|
||||
channel_roll (CH_1),
|
||||
channel_pitch (CH_2),
|
||||
channel_throttle (CH_3),
|
||||
channel_rudder (CH_4),
|
||||
rc_5 (CH_5),
|
||||
rc_6 (CH_6),
|
||||
rc_7 (CH_7),
|
||||
rc_8 (CH_8),
|
||||
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
|
||||
rc_9 (CH_9),
|
||||
rc_10 (CH_10),
|
||||
rc_11 (CH_11),
|
||||
#endif
|
||||
|
||||
// PID controller initial P initial I initial D initial imax
|
||||
//-----------------------------------------------------------------------------------
|
||||
pidNavRoll (NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE),
|
||||
|
|
|
@ -54,16 +54,7 @@ static void init_rc_out()
|
|||
APM_RC.enable_out(CH_2);
|
||||
APM_RC.enable_out(CH_3);
|
||||
APM_RC.enable_out(CH_4);
|
||||
APM_RC.enable_out(CH_5);
|
||||
APM_RC.enable_out(CH_6);
|
||||
APM_RC.enable_out(CH_7);
|
||||
APM_RC.enable_out(CH_8);
|
||||
|
||||
#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM1
|
||||
APM_RC.enable_out(CH_9);
|
||||
APM_RC.enable_out(CH_10);
|
||||
APM_RC.enable_out(CH_11);
|
||||
#endif
|
||||
enable_aux_servos();
|
||||
|
||||
APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs
|
||||
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim);
|
||||
|
|
|
@ -331,3 +331,15 @@ void RC_Channel::set_apm_rc( APM_RC_Class * apm_rc )
|
|||
{
|
||||
_apm_rc = apm_rc;
|
||||
}
|
||||
|
||||
void
|
||||
RC_Channel::output()
|
||||
{
|
||||
_apm_rc->OutputCh(_ch_out, radio_out);
|
||||
}
|
||||
|
||||
void
|
||||
RC_Channel::enable_out()
|
||||
{
|
||||
_apm_rc->enable_out(_ch_out);
|
||||
}
|
||||
|
|
|
@ -18,7 +18,7 @@ class RC_Channel{
|
|||
/// @param key EEPROM storage key for the channel trim parameters.
|
||||
/// @param name Optional name for the group.
|
||||
///
|
||||
RC_Channel() :
|
||||
RC_Channel(uint8_t ch_out) :
|
||||
radio_min (1100),
|
||||
radio_trim(1500),
|
||||
radio_max (1900),
|
||||
|
@ -26,7 +26,8 @@ class RC_Channel{
|
|||
_filter(false),
|
||||
_reverse(1),
|
||||
_dead_zone(0),
|
||||
_high(1) {}
|
||||
_high(1),
|
||||
_ch_out(ch_out) {}
|
||||
|
||||
// setup min and max radio values in CLI
|
||||
void update_min_max();
|
||||
|
@ -90,6 +91,8 @@ class RC_Channel{
|
|||
|
||||
float scale_output;
|
||||
static void set_apm_rc(APM_RC_Class * apm_rc);
|
||||
void output();
|
||||
void enable_out();
|
||||
static APM_RC_Class *_apm_rc;
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
@ -103,6 +106,7 @@ class RC_Channel{
|
|||
int16_t _low;
|
||||
int16_t _high_out;
|
||||
int16_t _low_out;
|
||||
uint8_t _ch_out;
|
||||
};
|
||||
|
||||
// This is ugly, but it fixes compilation on arduino
|
||||
|
|
|
@ -190,3 +190,14 @@ void update_aux_servo_function( RC_Channel_aux* rc_a,
|
|||
g_rc_function[RC_Channel_aux::k_cam_trigger]->angle_max / 10);
|
||||
G_RC_AUX(k_egg_drop)->set_range(0,100);
|
||||
}
|
||||
|
||||
/// Should be called after the the servo functions have been initialized
|
||||
void
|
||||
enable_aux_servos()
|
||||
{
|
||||
for (uint8_t i = 0; i < RC_Channel_aux::k_nr_aux_servo_functions ; i++)
|
||||
{
|
||||
if (g_rc_function[i]) g_rc_function[i]->enable_out();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -23,8 +23,8 @@ public:
|
|||
/// @param key EEPROM storage key for the channel trim parameters.
|
||||
/// @param name Optional name for the group.
|
||||
///
|
||||
RC_Channel_aux() :
|
||||
RC_Channel(),
|
||||
RC_Channel_aux(uint8_t ch_out) :
|
||||
RC_Channel(ch_out),
|
||||
function (0),
|
||||
angle_min (-4500), // assume -45 degrees min deflection
|
||||
angle_max (4500) // assume 45 degrees max deflection
|
||||
|
@ -67,6 +67,7 @@ public:
|
|||
};
|
||||
|
||||
void update_aux_servo_function(RC_Channel_aux* rc_a = NULL, RC_Channel_aux* rc_b = NULL, RC_Channel_aux* rc_c = NULL, RC_Channel_aux* rc_d = NULL, RC_Channel_aux* rc_e = NULL, RC_Channel_aux* rc_f = NULL, RC_Channel_aux* rc_g = NULL);
|
||||
void enable_aux_servos();
|
||||
extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
|
||||
|
||||
#endif /* RC_CHANNEL_AUX_H_ */
|
||||
|
|
Loading…
Reference in New Issue