Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: use new SRV_Channels API

This commit is contained in:
Andrew Tridgell 2017-01-07 12:06:40 +11:00
parent b83f50be0f
commit 5a87ae3f01
14 changed files with 95 additions and 216 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/ArduCopter.cpp
View File

@ -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();

4
ArduCopter/Copter.cpp
View File

@ -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),

3
ArduCopter/Copter.h
View File

@ -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);

44
ArduCopter/GCS_Mavlink.cpp
View File

@ -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;

58
ArduCopter/Parameters.cpp
View File

@ -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
};

66
ArduCopter/Parameters.h
View File

@ -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[];

2
ArduCopter/baro_ground_effect.cpp
View File

@ -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();

2
ArduCopter/heli.cpp
View File

@ -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:

8
ArduCopter/heli_control_acro.cpp
View File

@ -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

4
ArduCopter/motors.cpp
View File

@ -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) {

37
ArduCopter/radio.cpp
View File

@ -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;

12
ArduCopter/setup.cpp
View File

@ -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)

42
ArduCopter/switches.cpp
View File

@ -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);

23
ArduCopter/tuning.cpp
View File

@ -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) {
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: