mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-07 00:13:59 -04:00
Copter: use new SRV_Channels API
This commit is contained in:
Andrew Tridgell
parent
b83f50be0f
commit
5a87ae3f01
@ -500,7 +500,7 @@ void Copter::one_hz_loop()
|
||||
}
|
||||
|
||||
// update assigned functions and enable auxiliary servos
|
||||
RC_Channel_aux::enable_aux_servos();
|
||||
SRV_Channels::enable_aux_servos();
|
||||
|
||||
check_usb_mux();
|
||||
|
||||
|
||||
@ -28,11 +28,7 @@ Copter::Copter(void) :
|
||||
FUNCTOR_BIND_MEMBER(&Copter::verify_command_callback, bool, const AP_Mission::Mission_Command &),
|
||||
FUNCTOR_BIND_MEMBER(&Copter::exit_mission, void)),
|
||||
control_mode(STABILIZE),
|
||||
#if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments
|
||||
motors(g.rc_7, MAIN_LOOP_RATE),
|
||||
#else
|
||||
motors(MAIN_LOOP_RATE),
|
||||
#endif
|
||||
scaleLongDown(1),
|
||||
wp_bearing(0),
|
||||
home_bearing(0),
|
||||
|
||||
@ -694,7 +694,6 @@ private:
|
||||
void send_nav_controller_output(mavlink_channel_t chan);
|
||||
void send_simstate(mavlink_channel_t chan);
|
||||
void send_hwstatus(mavlink_channel_t chan);
|
||||
void send_servo_out(mavlink_channel_t chan);
|
||||
void send_vfr_hud(mavlink_channel_t chan);
|
||||
void send_current_waypoint(mavlink_channel_t chan);
|
||||
void send_rangefinder(mavlink_channel_t chan);
|
||||
@ -1069,7 +1068,7 @@ private:
|
||||
bool check_if_auxsw_mode_used(uint8_t auxsw_mode_check);
|
||||
bool check_duplicate_auxsw(void);
|
||||
void reset_control_switch();
|
||||
uint8_t read_3pos_switch(int16_t radio_in);
|
||||
uint8_t read_3pos_switch(uint8_t chan);
|
||||
void read_aux_switches();
|
||||
void init_aux_switches();
|
||||
void init_aux_switch_function(int8_t ch_option, uint8_t ch_flag);
|
||||
|
||||
@ -190,44 +190,6 @@ void NOINLINE Copter::send_hwstatus(mavlink_channel_t chan)
|
||||
0);
|
||||
}
|
||||
|
||||
void NOINLINE Copter::send_servo_out(mavlink_channel_t chan)
|
||||
{
|
||||
#if HIL_MODE != HIL_MODE_DISABLED
|
||||
// normalized values scaled to -10000 to 10000
|
||||
// This is used for HIL. Do not change without discussing with HIL maintainers
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
mavlink_msg_rc_channels_scaled_send(
|
||||
chan,
|
||||
millis(),
|
||||
0, // port 0
|
||||
g.rc_1.get_servo_out(),
|
||||
g.rc_2.get_servo_out(),
|
||||
g.rc_3.get_radio_out(),
|
||||
g.rc_4.get_servo_out(),
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
receiver_rssi);
|
||||
#else
|
||||
mavlink_msg_rc_channels_scaled_send(
|
||||
chan,
|
||||
millis(),
|
||||
0, // port 0
|
||||
g.rc_1.get_servo_out(),
|
||||
g.rc_2.get_servo_out(),
|
||||
g.rc_3.get_radio_out(),
|
||||
g.rc_4.get_servo_out(),
|
||||
10000 * g.rc_1.norm_output(),
|
||||
10000 * g.rc_2.norm_output(),
|
||||
10000 * g.rc_3.norm_output(),
|
||||
10000 * g.rc_4.norm_output(),
|
||||
receiver_rssi);
|
||||
#endif
|
||||
#endif // HIL_MODE
|
||||
}
|
||||
|
||||
void NOINLINE Copter::send_vfr_hud(mavlink_channel_t chan)
|
||||
{
|
||||
mavlink_msg_vfr_hud_send(
|
||||
@ -443,11 +405,6 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
|
||||
send_system_time(copter.gps);
|
||||
break;
|
||||
|
||||
case MSG_SERVO_OUT:
|
||||
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
|
||||
copter.send_servo_out(chan);
|
||||
break;
|
||||
|
||||
case MSG_RADIO_IN:
|
||||
CHECK_PAYLOAD_SIZE(RC_CHANNELS);
|
||||
send_radio_in(copter.receiver_rssi);
|
||||
@ -585,6 +542,7 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
|
||||
case MSG_LIMITS_STATUS:
|
||||
case MSG_WIND:
|
||||
case MSG_POSITION_TARGET_GLOBAL_INT:
|
||||
case MSG_SERVO_OUT:
|
||||
// unused
|
||||
break;
|
||||
|
||||
|
||||
@ -513,56 +513,6 @@ const AP_Param::Info Copter::var_info[] = {
|
||||
// @User: Advanced
|
||||
GSCALAR(fs_crash_check, "FS_CRASH_CHECK", 1),
|
||||
|
||||
// RC channel
|
||||
//-----------
|
||||
// @Group: RC1_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(rc_1, "RC1_", RC_Channel),
|
||||
// @Group: RC2_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(rc_2, "RC2_", RC_Channel),
|
||||
// @Group: RC3_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(rc_3, "RC3_", RC_Channel),
|
||||
// @Group: RC4_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(rc_4, "RC4_", RC_Channel),
|
||||
// @Group: RC5_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
|
||||
GGROUP(rc_5, "RC5_", RC_Channel_aux),
|
||||
// @Group: RC6_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
|
||||
GGROUP(rc_6, "RC6_", RC_Channel_aux),
|
||||
// @Group: RC7_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
|
||||
GGROUP(rc_7, "RC7_", RC_Channel_aux),
|
||||
// @Group: RC8_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
|
||||
GGROUP(rc_8, "RC8_", RC_Channel_aux),
|
||||
|
||||
// @Group: RC9_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
|
||||
GGROUP(rc_9, "RC9_", RC_Channel_aux),
|
||||
|
||||
// @Group: RC10_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
|
||||
GGROUP(rc_10, "RC10_", RC_Channel_aux),
|
||||
// @Group: RC11_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
|
||||
GGROUP(rc_11, "RC11_", RC_Channel_aux),
|
||||
|
||||
// @Group: RC12_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
|
||||
GGROUP(rc_12, "RC12_", RC_Channel_aux),
|
||||
|
||||
// @Group: RC13_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
|
||||
GGROUP(rc_13, "RC13_", RC_Channel_aux),
|
||||
|
||||
// @Group: RC14_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
|
||||
GGROUP(rc_14, "RC14_", RC_Channel_aux),
|
||||
|
||||
// @Param: RC_SPEED
|
||||
// @DisplayName: ESC Update Speed
|
||||
// @Description: This is the speed in Hertz that your ESCs will receive updates
|
||||
@ -1052,6 +1002,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("FRAME_CLASS", 15, ParametersG2, frame_class, 0),
|
||||
|
||||
// @Group: SERVO
|
||||
// @Path: ../libraries/SRV_Channel/SRV_Channel.cpp
|
||||
AP_SUBGROUPINFO(servo_channels, "SERVO", 16, ParametersG2, SRV_Channels),
|
||||
|
||||
// @Group: RC
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
AP_SUBGROUPINFO(rc_channels, "RC", 17, ParametersG2, RC_Channels),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
@ -1065,7 +1023,7 @@ ParametersG2::ParametersG2(void)
|
||||
, proximity(copter.serial_manager)
|
||||
#endif
|
||||
#if ADVANCED_FAILSAFE == ENABLED
|
||||
,afs(copter.mission, copter.barometer, copter.gps, copter.rcmap)
|
||||
,afs(copter.mission, copter.barometer, copter.gps, copter.rcmap)
|
||||
#endif
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
|
||||
@ -113,8 +113,8 @@ public:
|
||||
k_param_rc_feel_rp,
|
||||
k_param_NavEKF, // deprecated - remove
|
||||
k_param_mission, // mission library
|
||||
k_param_rc_13,
|
||||
k_param_rc_14,
|
||||
k_param_rc_13_old,
|
||||
k_param_rc_14_old,
|
||||
k_param_rally,
|
||||
k_param_poshold_brake_rate,
|
||||
k_param_poshold_brake_angle_max,
|
||||
@ -273,16 +273,16 @@ public:
|
||||
//
|
||||
// 170: Radio settings
|
||||
//
|
||||
k_param_rc_1 = 170,
|
||||
k_param_rc_2,
|
||||
k_param_rc_3,
|
||||
k_param_rc_4,
|
||||
k_param_rc_5,
|
||||
k_param_rc_6,
|
||||
k_param_rc_7,
|
||||
k_param_rc_8,
|
||||
k_param_rc_10,
|
||||
k_param_rc_11,
|
||||
k_param_rc_1_old = 170,
|
||||
k_param_rc_2_old,
|
||||
k_param_rc_3_old,
|
||||
k_param_rc_4_old,
|
||||
k_param_rc_5_old,
|
||||
k_param_rc_6_old,
|
||||
k_param_rc_7_old,
|
||||
k_param_rc_8_old,
|
||||
k_param_rc_10_old,
|
||||
k_param_rc_11_old,
|
||||
k_param_throttle_min, // remove
|
||||
k_param_throttle_max, // remove
|
||||
k_param_failsafe_throttle,
|
||||
@ -297,8 +297,8 @@ public:
|
||||
k_param_failsafe_battery_enabled,
|
||||
k_param_throttle_mid, // remove
|
||||
k_param_failsafe_gps_enabled, // remove
|
||||
k_param_rc_9,
|
||||
k_param_rc_12,
|
||||
k_param_rc_9_old,
|
||||
k_param_rc_12_old,
|
||||
k_param_failsafe_gcs,
|
||||
k_param_rcmap, // 199
|
||||
|
||||
@ -460,22 +460,6 @@ public:
|
||||
AP_Int8 throw_motor_start;
|
||||
AP_Int8 terrain_follow;
|
||||
|
||||
// RC channels
|
||||
RC_Channel rc_1;
|
||||
RC_Channel rc_2;
|
||||
RC_Channel rc_3;
|
||||
RC_Channel rc_4;
|
||||
RC_Channel_aux rc_5;
|
||||
RC_Channel_aux rc_6;
|
||||
RC_Channel_aux rc_7;
|
||||
RC_Channel_aux rc_8;
|
||||
RC_Channel_aux rc_9;
|
||||
RC_Channel_aux rc_10;
|
||||
RC_Channel_aux rc_11;
|
||||
RC_Channel_aux rc_12;
|
||||
RC_Channel_aux rc_13;
|
||||
RC_Channel_aux rc_14;
|
||||
|
||||
AP_Int16 rc_speed; // speed of fast RC Channels in Hz
|
||||
|
||||
// Acro parameters
|
||||
@ -503,22 +487,6 @@ public:
|
||||
// Note: keep initializers here in the same order as they are declared
|
||||
// above.
|
||||
Parameters() :
|
||||
|
||||
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),
|
||||
rc_9 (CH_9),
|
||||
rc_10 (CH_10),
|
||||
rc_11 (CH_11),
|
||||
rc_12 (CH_12),
|
||||
rc_13 (CH_13),
|
||||
rc_14 (CH_14),
|
||||
|
||||
// PID controller initial P initial I initial D initial imax initial filt hz pid rate
|
||||
//---------------------------------------------------------------------------------------------------------------------------------
|
||||
pi_vel_xy (VEL_XY_P, VEL_XY_I, VEL_XY_IMAX, VEL_XY_FILT_HZ, WPNAV_LOITER_UPDATE_TIME),
|
||||
@ -590,6 +558,12 @@ public:
|
||||
|
||||
// frame class
|
||||
AP_Int8 frame_class;
|
||||
|
||||
// RC input channels
|
||||
RC_Channels rc_channels;
|
||||
|
||||
// control over servo output ranges
|
||||
SRV_Channels servo_channels;
|
||||
};
|
||||
|
||||
extern const AP_Param::Info var_info[];
|
||||
|
||||
@ -37,7 +37,7 @@ void Copter::update_ground_effect_detector(void)
|
||||
}
|
||||
|
||||
// if we aren't taking off yet, reset the takeoff timer, altitude and complete flag
|
||||
bool throttle_up = mode_has_manual_throttle(control_mode) && g.rc_3.get_control_in() > 0;
|
||||
bool throttle_up = mode_has_manual_throttle(control_mode) && channel_throttle->get_control_in() > 0;
|
||||
if (!throttle_up && ap.land_complete) {
|
||||
gndeffect_state.takeoff_time_ms = tnow_ms;
|
||||
gndeffect_state.takeoff_alt_cm = inertial_nav.get_altitude();
|
||||
|
||||
@ -139,7 +139,7 @@ void Copter::heli_update_rotor_speed_targets()
|
||||
// get rotor control method
|
||||
uint8_t rsc_control_mode = motors.get_rsc_mode();
|
||||
|
||||
float rsc_control_deglitched = rotor_speed_deglitch_filter.apply((float)g.rc_8.get_control_in()) * 0.001f;
|
||||
float rsc_control_deglitched = rotor_speed_deglitch_filter.apply((float)RC_Channels::rc_channel(CH_8)->get_control_in()) * 0.001f;
|
||||
|
||||
switch (rsc_control_mode) {
|
||||
case ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH:
|
||||
|
||||
@ -58,7 +58,7 @@ void Copter::heli_acro_run()
|
||||
// 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);
|
||||
target_yaw = channel_yaw->get_control_in_zero_dz();
|
||||
}
|
||||
|
||||
// run attitude controller
|
||||
@ -68,15 +68,15 @@ void Copter::heli_acro_run()
|
||||
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 roll_in = channel_roll->get_control_in_zero_dz();
|
||||
float pitch_in = channel_pitch->get_control_in_zero_dz();
|
||||
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);
|
||||
yaw_in = channel_yaw->get_control_in_zero_dz();
|
||||
} else {
|
||||
// if there is no external gyro then run the usual
|
||||
// ACRO_YAW_P gain on the input control, including
|
||||
|
||||
@ -101,8 +101,8 @@ void Copter::auto_disarm_check()
|
||||
if (mode_has_manual_throttle(control_mode) || !sprung_throttle_stick) {
|
||||
thr_low = ap.throttle_zero;
|
||||
} else {
|
||||
float deadband_top = g.rc_3.get_control_mid() + g.throttle_deadzone;
|
||||
thr_low = g.rc_3.get_control_in() <= deadband_top;
|
||||
float deadband_top = channel_throttle->get_control_mid() + g.throttle_deadzone;
|
||||
thr_low = channel_throttle->get_control_in() <= deadband_top;
|
||||
}
|
||||
|
||||
if (!thr_low || !ap.land_complete) {
|
||||
|
||||
@ -11,36 +11,32 @@ void Copter::default_dead_zones()
|
||||
#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);
|
||||
RC_Channels::rc_channel(CH_6)->set_default_dead_zone(10);
|
||||
#else
|
||||
channel_throttle->set_default_dead_zone(30);
|
||||
channel_yaw->set_default_dead_zone(10);
|
||||
#endif
|
||||
g.rc_6.set_default_dead_zone(0);
|
||||
RC_Channels::rc_channel(CH_6)->set_default_dead_zone(0);
|
||||
}
|
||||
|
||||
void Copter::init_rc_in()
|
||||
{
|
||||
channel_roll = RC_Channel::rc_channel(rcmap.roll()-1);
|
||||
channel_pitch = RC_Channel::rc_channel(rcmap.pitch()-1);
|
||||
channel_throttle = RC_Channel::rc_channel(rcmap.throttle()-1);
|
||||
channel_yaw = RC_Channel::rc_channel(rcmap.yaw()-1);
|
||||
channel_roll = RC_Channels::rc_channel(rcmap.roll()-1);
|
||||
channel_pitch = RC_Channels::rc_channel(rcmap.pitch()-1);
|
||||
channel_throttle = RC_Channels::rc_channel(rcmap.throttle()-1);
|
||||
channel_yaw = RC_Channels::rc_channel(rcmap.yaw()-1);
|
||||
|
||||
// set rc channel ranges
|
||||
channel_roll->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
|
||||
channel_pitch->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
|
||||
channel_yaw->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
|
||||
channel_throttle->set_range(0, 1000);
|
||||
|
||||
channel_roll->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
|
||||
channel_pitch->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
|
||||
channel_yaw->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
|
||||
channel_throttle->set_range(1000);
|
||||
|
||||
//set auxiliary servo ranges
|
||||
g.rc_5.set_range_in(0,1000);
|
||||
g.rc_6.set_range_in(0,1000);
|
||||
g.rc_7.set_range_in(0,1000);
|
||||
g.rc_8.set_range_in(0,1000);
|
||||
RC_Channels::rc_channel(CH_5)->set_range(1000);
|
||||
RC_Channels::rc_channel(CH_6)->set_range(1000);
|
||||
RC_Channels::rc_channel(CH_7)->set_range(1000);
|
||||
RC_Channels::rc_channel(CH_8)->set_range(1000);
|
||||
|
||||
// set default dead zones
|
||||
default_dead_zones();
|
||||
@ -67,9 +63,6 @@ void Copter::init_rc_out()
|
||||
read_radio();
|
||||
}
|
||||
|
||||
// we want the input to be scaled correctly
|
||||
channel_throttle->set_range_out(0,1000);
|
||||
|
||||
// setup correct scaling for ESCs like the UAVCAN PX4ESC which
|
||||
// take a proportion of speed.
|
||||
hal.rcout->set_esc_scaling(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
|
||||
@ -84,7 +77,7 @@ void Copter::init_rc_out()
|
||||
}
|
||||
|
||||
// refresh auxiliary channel to function map
|
||||
RC_Channel_aux::update_aux_servo_function();
|
||||
SRV_Channels::update_aux_servo_function();
|
||||
|
||||
#if FRAME_CONFIG != HELI_FRAME
|
||||
/*
|
||||
@ -110,7 +103,7 @@ void Copter::read_radio()
|
||||
|
||||
if (hal.rcin->new_input()) {
|
||||
ap.new_radio_frame = true;
|
||||
RC_Channel::set_pwm_all();
|
||||
RC_Channels::set_pwm_all();
|
||||
|
||||
set_throttle_and_failsafe(channel_throttle->get_radio_in());
|
||||
set_throttle_zero_flag(channel_throttle->get_control_in());
|
||||
@ -121,13 +114,13 @@ void Copter::read_radio()
|
||||
}
|
||||
|
||||
// update output on any aux channels, for manual passthru
|
||||
RC_Channel_aux::output_ch_all();
|
||||
SRV_Channels::output_ch_all();
|
||||
|
||||
// pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters)
|
||||
radio_passthrough_to_motors();
|
||||
|
||||
float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f;
|
||||
rc_throttle_control_in_filter.apply(g.rc_3.get_control_in(), dt);
|
||||
rc_throttle_control_in_filter.apply(channel_throttle->get_control_in(), dt);
|
||||
last_radio_update_ms = tnow_ms;
|
||||
}else{
|
||||
uint32_t elapsed = tnow_ms - last_radio_update_ms;
|
||||
|
||||
@ -369,14 +369,10 @@ void Copter::report_optflow()
|
||||
|
||||
void Copter::print_radio_values()
|
||||
{
|
||||
cliSerial->printf("CH1: %d | %d\n", (int)channel_roll->get_radio_min(), (int)channel_roll->get_radio_max());
|
||||
cliSerial->printf("CH2: %d | %d\n", (int)channel_pitch->get_radio_min(), (int)channel_pitch->get_radio_max());
|
||||
cliSerial->printf("CH3: %d | %d\n", (int)channel_throttle->get_radio_min(), (int)channel_throttle->get_radio_max());
|
||||
cliSerial->printf("CH4: %d | %d\n", (int)channel_yaw->get_radio_min(), (int)channel_yaw->get_radio_max());
|
||||
cliSerial->printf("CH5: %d | %d\n", (int)g.rc_5.get_radio_min(), (int)g.rc_5.get_radio_max());
|
||||
cliSerial->printf("CH6: %d | %d\n", (int)g.rc_6.get_radio_min(), (int)g.rc_6.get_radio_max());
|
||||
cliSerial->printf("CH7: %d | %d\n", (int)g.rc_7.get_radio_min(), (int)g.rc_7.get_radio_max());
|
||||
cliSerial->printf("CH8: %d | %d\n", (int)g.rc_8.get_radio_min(), (int)g.rc_8.get_radio_max());
|
||||
for (uint8_t i=0; i<8; i++) {
|
||||
RC_Channel *rc = RC_Channels::rc_channel(i);
|
||||
cliSerial->printf("CH%u: %d | %d\n", (unsigned)i, rc->get_radio_min(), rc->get_radio_max());
|
||||
}
|
||||
}
|
||||
|
||||
void Copter::print_switch(uint8_t p, uint8_t m, bool b)
|
||||
|
||||
@ -19,11 +19,12 @@ void Copter::read_control_switch()
|
||||
|
||||
// calculate position of flight mode switch
|
||||
int8_t switch_position;
|
||||
if (g.rc_5.get_radio_in() < 1231) switch_position = 0;
|
||||
else if (g.rc_5.get_radio_in() < 1361) switch_position = 1;
|
||||
else if (g.rc_5.get_radio_in() < 1491) switch_position = 2;
|
||||
else if (g.rc_5.get_radio_in() < 1621) switch_position = 3;
|
||||
else if (g.rc_5.get_radio_in() < 1750) switch_position = 4;
|
||||
uint16_t rc5_in = RC_Channels::rc_channel(CH_5)->get_radio_in();
|
||||
if (rc5_in < 1231) switch_position = 0;
|
||||
else if (rc5_in < 1361) switch_position = 1;
|
||||
else if (rc5_in < 1491) switch_position = 2;
|
||||
else if (rc5_in < 1621) switch_position = 3;
|
||||
else if (rc5_in < 1750) switch_position = 4;
|
||||
else switch_position = 5;
|
||||
|
||||
// store time that switch last moved
|
||||
@ -107,17 +108,18 @@ void Copter::reset_control_switch()
|
||||
}
|
||||
|
||||
// read_3pos_switch
|
||||
uint8_t Copter::read_3pos_switch(int16_t radio_in)
|
||||
uint8_t Copter::read_3pos_switch(uint8_t chan)
|
||||
{
|
||||
uint16_t radio_in = RC_Channels::rc_channel(chan)->get_radio_in();
|
||||
if (radio_in < AUX_SWITCH_PWM_TRIGGER_LOW) return AUX_SWITCH_LOW; // switch is in low position
|
||||
if (radio_in > AUX_SWITCH_PWM_TRIGGER_HIGH) return AUX_SWITCH_HIGH; // switch is in high position
|
||||
return AUX_SWITCH_MIDDLE; // switch is in middle position
|
||||
}
|
||||
|
||||
// can't take reference to a bitfield member, thus a #define:
|
||||
#define read_aux_switch(rc, flag, option) \
|
||||
#define read_aux_switch(chan, flag, option) \
|
||||
do { \
|
||||
switch_position = read_3pos_switch(rc.get_radio_in()); \
|
||||
switch_position = read_3pos_switch(chan); \
|
||||
if (flag != switch_position) { \
|
||||
flag = switch_position; \
|
||||
do_aux_switch_function(option, flag); \
|
||||
@ -134,14 +136,14 @@ void Copter::read_aux_switches()
|
||||
return;
|
||||
}
|
||||
|
||||
read_aux_switch(g.rc_7, aux_con.CH7_flag, g.ch7_option);
|
||||
read_aux_switch(g.rc_8, aux_con.CH8_flag, g.ch8_option);
|
||||
read_aux_switch(g.rc_9, aux_con.CH9_flag, g.ch9_option);
|
||||
read_aux_switch(g.rc_10, aux_con.CH10_flag, g.ch10_option);
|
||||
read_aux_switch(g.rc_11, aux_con.CH11_flag, g.ch11_option);
|
||||
read_aux_switch(CH_7, aux_con.CH7_flag, g.ch7_option);
|
||||
read_aux_switch(CH_8, aux_con.CH8_flag, g.ch8_option);
|
||||
read_aux_switch(CH_9, aux_con.CH9_flag, g.ch9_option);
|
||||
read_aux_switch(CH_10, aux_con.CH10_flag, g.ch10_option);
|
||||
read_aux_switch(CH_11, aux_con.CH11_flag, g.ch11_option);
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
||||
read_aux_switch(g.rc_12, aux_con.CH12_flag, g.ch12_option);
|
||||
read_aux_switch(CH_12, aux_con.CH12_flag, g.ch12_option);
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -151,14 +153,14 @@ void Copter::read_aux_switches()
|
||||
void Copter::init_aux_switches()
|
||||
{
|
||||
// set the CH7 ~ CH12 flags
|
||||
aux_con.CH7_flag = read_3pos_switch(g.rc_7.get_radio_in());
|
||||
aux_con.CH8_flag = read_3pos_switch(g.rc_8.get_radio_in());
|
||||
aux_con.CH10_flag = read_3pos_switch(g.rc_10.get_radio_in());
|
||||
aux_con.CH11_flag = read_3pos_switch(g.rc_11.get_radio_in());
|
||||
aux_con.CH7_flag = read_3pos_switch(CH_7);
|
||||
aux_con.CH8_flag = read_3pos_switch(CH_8);
|
||||
aux_con.CH10_flag = read_3pos_switch(CH_10);
|
||||
aux_con.CH11_flag = read_3pos_switch(CH_11);
|
||||
|
||||
// ch9, ch12 only supported on some boards
|
||||
aux_con.CH9_flag = read_3pos_switch(g.rc_9.get_radio_in());
|
||||
aux_con.CH12_flag = read_3pos_switch(g.rc_12.get_radio_in());
|
||||
aux_con.CH9_flag = read_3pos_switch(CH_9);
|
||||
aux_con.CH12_flag = read_3pos_switch(CH_12);
|
||||
|
||||
// initialise functions assigned to switches
|
||||
init_aux_switch_function(g.ch7_option, aux_con.CH7_flag);
|
||||
|
||||
@ -8,19 +8,22 @@
|
||||
// tuning - updates parameters based on the ch6 tuning knob's position
|
||||
// should be called at 3.3hz
|
||||
void Copter::tuning() {
|
||||
RC_Channel *rc6 = RC_Channels::rc_channel(CH_6);
|
||||
|
||||
// exit immediately if not using tuning function, or when radio failsafe is invoked, so tuning values are not set to zero
|
||||
if ((g.radio_tuning <= 0) || failsafe.radio || failsafe.radio_counter != 0 || g.rc_6.get_radio_in() == 0) {
|
||||
// exit immediately if not using tuning function, or when radio failsafe is invoked, so tuning values are not set to zero
|
||||
if ((g.radio_tuning <= 0) || failsafe.radio || failsafe.radio_counter != 0 || rc6->get_radio_in() == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// set tuning range and then get new value
|
||||
g.rc_6.set_range_in(g.radio_tuning_low,g.radio_tuning_high);
|
||||
float tuning_value = (float)g.rc_6.get_control_in() / 1000.0f;
|
||||
uint16_t radio_in = rc6->get_radio_in();
|
||||
float v = constrain_float((radio_in - rc6->get_radio_min()) / float(rc6->get_radio_max() - rc6->get_radio_min()), 0, 1);
|
||||
int16_t control_in = g.radio_tuning_low + v * (g.radio_tuning_high - g.radio_tuning_low);
|
||||
float tuning_value = control_in / 1000.0f;
|
||||
|
||||
// Tuning Value should never be outside the bounds of the specified low and high value
|
||||
tuning_value = constrain_float(tuning_value, g.radio_tuning_low/1000.0f, g.radio_tuning_high/1000.0f);
|
||||
|
||||
Log_Write_Parameter_Tuning(g.radio_tuning, tuning_value, g.rc_6.get_control_in(), g.radio_tuning_low, g.radio_tuning_high);
|
||||
Log_Write_Parameter_Tuning(g.radio_tuning, tuning_value, control_in, g.radio_tuning_low, g.radio_tuning_high);
|
||||
|
||||
switch(g.radio_tuning) {
|
||||
|
||||
@ -94,7 +97,7 @@ void Copter::tuning() {
|
||||
|
||||
case TUNING_WP_SPEED:
|
||||
// set waypoint navigation horizontal speed to 0 ~ 1000 cm/s
|
||||
wp_nav.set_speed_xy(g.rc_6.get_control_in());
|
||||
wp_nav.set_speed_xy(control_in);
|
||||
break;
|
||||
|
||||
// Acro roll pitch gain
|
||||
@ -109,7 +112,7 @@ void Copter::tuning() {
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
case TUNING_HELI_EXTERNAL_GYRO:
|
||||
motors.ext_gyro_gain((float)g.rc_6.get_control_in() / 1000.0f);
|
||||
motors.ext_gyro_gain((float)control_in / 1000.0f);
|
||||
break;
|
||||
|
||||
case TUNING_RATE_PITCH_FF:
|
||||
@ -127,12 +130,12 @@ void Copter::tuning() {
|
||||
|
||||
case TUNING_DECLINATION:
|
||||
// set declination to +-20degrees
|
||||
compass.set_declination(ToRad((2.0f * g.rc_6.get_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
|
||||
compass.set_declination(ToRad((2.0f * 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
|
||||
break;
|
||||
|
||||
case TUNING_CIRCLE_RATE:
|
||||
// set circle rate up to approximately 45 deg/sec in either direction
|
||||
circle_nav.set_rate((float)g.rc_6.get_control_in()/25.0f-20.0f);
|
||||
circle_nav.set_rate((float)control_in/25.0f-20.0f);
|
||||
break;
|
||||
|
||||
case TUNING_RANGEFINDER_GAIN:
|
||||
@ -173,7 +176,7 @@ void Copter::tuning() {
|
||||
|
||||
case TUNING_RC_FEEL_RP:
|
||||
// roll-pitch input smoothing
|
||||
g.rc_feel_rp = g.rc_6.get_control_in() / 10;
|
||||
g.rc_feel_rp = control_in / 10;
|
||||
break;
|
||||
|
||||
case TUNING_RATE_PITCH_KP:
|
||||
|
||||
Loading…
Reference in New Issue
Block a user