zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Plane: added in-flight transmitter tuning

master
Andrew Tridgell 9 years ago
parent
9020a34a34
commit
27fb35253c
9 changed files with 318 additions and 2 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      ArduPlane/Log.cpp
  2. 4
      ArduPlane/Parameters.cpp
  3. 1
      ArduPlane/Parameters.h
  4. 7
      ArduPlane/Plane.h
  5. 3
      ArduPlane/defines.h
  6. 1
      ArduPlane/quadplane.h
  7. 2
      ArduPlane/radio.cpp
  8. 223
      ArduPlane/tuning.cpp
  9. 75
      ArduPlane/tuning.h

4
ArduPlane/Log.cpp
Unescape View File

@ -482,7 +482,7 @@ void Plane::Log_Write_Home_And_Origin() @@ -482,7 +482,7 @@ void Plane::Log_Write_Home_And_Origin()
}
}
static const struct LogStructure log_structure[] = {
const struct LogStructure Plane::log_structure[] = {
LOG_COMMON_STRUCTURES,
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
"PM", "QIHIhhhBH", "TimeUS,LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr" },
@ -506,6 +506,8 @@ static const struct LogStructure log_structure[] = { @@ -506,6 +506,8 @@ static const struct LogStructure log_structure[] = {
{ LOG_OPTFLOW_MSG, sizeof(log_Optflow),
"OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY" },
#endif
{ LOG_PARAMTUNE_MSG, sizeof(Tuning::log_ParameterTuning),
"PTUN", "QBfff", "TimeUS,Param,TunVal,TunLo,TunHi" },
TECS_LOG_FORMAT(LOG_TECS_MSG)
};

4
ArduPlane/Parameters.cpp
Unescape View File

@ -1106,6 +1106,10 @@ const AP_Param::Info Plane::var_info[] = { @@ -1106,6 +1106,10 @@ const AP_Param::Info Plane::var_info[] = {
// @Path: quadplane.cpp
GOBJECT(quadplane, "Q_", QuadPlane),
// @Group: TUNE_
// @Path: tuning.cpp
GOBJECT(tuning, "TUNE_", Tuning),
// @Group: Q_A_
// @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
{ AP_PARAM_GROUP, "Q_A_", Parameters::k_param_q_attitude_control,

1
ArduPlane/Parameters.h
Unescape View File

@ -272,6 +272,7 @@ public: @@ -272,6 +272,7 @@ public:
k_param_long_fs_timeout,
k_param_rc_13,
k_param_rc_14,
k_param_tuning,
//
// 200: Feed-forward gains

7
ArduPlane/Plane.h
Unescape View File

@ -95,6 +95,7 @@ @@ -95,6 +95,7 @@
#include <AP_ADSB/AP_ADSB.h>
#include "quadplane.h"
#include "tuning.h"
// Configuration
#include "config.h"
@ -136,6 +137,7 @@ public: @@ -136,6 +137,7 @@ public:
friend class Parameters;
friend class AP_Arming_Plane;
friend class QuadPlane;
friend class Tuning;
Plane(void);
@ -729,6 +731,11 @@ private: @@ -729,6 +731,11 @@ private:
// support for quadcopter-plane
QuadPlane quadplane{ahrs};
// support for transmitter tuning
Tuning tuning;
static const struct LogStructure log_structure[];
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
// the crc of the last created PX4Mixer
int32_t last_mixer_crc = -1;

3
ArduPlane/defines.h
Unescape View File

@ -117,7 +117,8 @@ enum log_messages { @@ -117,7 +117,8 @@ enum log_messages {
LOG_ARM_DISARM_MSG,
LOG_STATUS_MSG,
LOG_OPTFLOW_MSG,
LOG_QTUN_MSG
LOG_QTUN_MSG,
LOG_PARAMTUNE_MSG
};
#define MASK_LOG_ATTITUDE_FAST (1<<0)

1
ArduPlane/quadplane.h
Unescape View File

@ -14,6 +14,7 @@ class QuadPlane @@ -14,6 +14,7 @@ class QuadPlane
{
public:
friend class Plane;
friend class Tuning;
QuadPlane(AP_AHRS_NavEKF &_ahrs);
// var_info for holding Parameter information

2
ArduPlane/radio.cpp
Unescape View File

@ -215,6 +215,8 @@ void Plane::read_radio() @@ -215,6 +215,8 @@ void Plane::read_radio()
} else {
rudder_input = channel_rudder->control_in;
}
tuning.check_input();
}
void Plane::control_failsafe(uint16_t pwm)

223
ArduPlane/tuning.cpp
Unescape View File

@ -0,0 +1,223 @@ @@ -0,0 +1,223 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Plane.h"
const AP_Param::GroupInfo Tuning::var_info[] = {
// @Param: CHAN
// @DisplayName: Transmitter tuning channel
// @Description: This sets the channel for transmitter tuning
// @Values: 0:Disable,1:Chan1,2:Chan3,3:Chan3,4:Chan4,5:Chan5,6:Chan6,7:Chan7,8:Chan8,9:Chan9,10:Chan10,11:Chan11,12:Chan12,13:Chan13,14:Chan14,15:Chan15,16:Chan16
// @User: Standard
AP_GROUPINFO_FLAGS("CHAN", 1, Tuning, channel, 0, AP_PARAM_FLAG_ENABLE),
// @Param: PARM
// @DisplayName: Transmitter tuning parameter
// @Description: This sets which parameter or combination of parameters will be tuned
// @User: Standard
AP_GROUPINFO("PARM", 2, Tuning, parm, 0),
// @Param: MIN
// @DisplayName: Transmitter tuning minimum value
// @Description: This sets the minimum value
// @User: Standard
AP_GROUPINFO("MIN", 3, Tuning, minimum, 0),
// @Param: MAX
// @DisplayName: Transmitter tuning maximum value
// @Description: This sets the maximum value
// @User: Standard
AP_GROUPINFO("MAX", 4, Tuning, maximum, 0),
AP_GROUPEND
};
/*
constructor
*/
Tuning::Tuning(void)
{
AP_Param::setup_object_defaults(this, var_info);
}
/*
check for changed tuning input
*/
void Tuning::check_input(void)
{
if (channel <= 0 || parm <= 0) {
// disabled
return;
}
// only adjust values at 4Hz
uint32_t now = AP_HAL::millis();
if (now - last_check_ms < 250) {
return;
}
last_check_ms = now;
if (channel > hal.rcin->num_channels()) {
return;
}
RC_Channel *chan = RC_Channel::rc_channel(channel-1);
if (chan == nullptr) {
return;
}
uint8_t input = chan->percent_input();
if (input == last_input_pct) {
// no change
return;
}
last_input_pct = input;
float tuning_value = minimum + (maximum-minimum)*(input*0.01f);
if (!plane.quadplane.available()) {
// quadplane tuning options not available
return;
}
switch((enum tuning_func)parm.get()) {
case TUNING_Q_RATE_ROLL_PITCH_KPI:
plane.quadplane.attitude_control->get_rate_roll_pid().kP(tuning_value);
plane.quadplane.attitude_control->get_rate_pitch_pid().kP(tuning_value);
plane.quadplane.attitude_control->get_rate_roll_pid().kI(tuning_value);
plane.quadplane.attitude_control->get_rate_pitch_pid().kI(tuning_value);
break;
case TUNING_Q_RATE_ROLL_PITCH_KP:
plane.quadplane.attitude_control->get_rate_roll_pid().kP(tuning_value);
plane.quadplane.attitude_control->get_rate_pitch_pid().kP(tuning_value);
break;
case TUNING_Q_RATE_ROLL_PITCH_KI:
plane.quadplane.attitude_control->get_rate_roll_pid().kI(tuning_value);
plane.quadplane.attitude_control->get_rate_pitch_pid().kI(tuning_value);
break;
case TUNING_Q_RATE_ROLL_PITCH_KD:
plane.quadplane.attitude_control->get_rate_roll_pid().kD(tuning_value);
plane.quadplane.attitude_control->get_rate_pitch_pid().kD(tuning_value);
break;
case TUNING_Q_RATE_ROLL_KPI:
plane.quadplane.attitude_control->get_rate_roll_pid().kP(tuning_value);
plane.quadplane.attitude_control->get_rate_roll_pid().kI(tuning_value);
break;
case TUNING_Q_RATE_ROLL_KP:
plane.quadplane.attitude_control->get_rate_roll_pid().kP(tuning_value);
break;
case TUNING_Q_RATE_ROLL_KI:
plane.quadplane.attitude_control->get_rate_roll_pid().kI(tuning_value);
break;
case TUNING_Q_RATE_ROLL_KD:
plane.quadplane.attitude_control->get_rate_roll_pid().kD(tuning_value);
break;
case TUNING_Q_RATE_PITCH_KPI:
plane.quadplane.attitude_control->get_rate_pitch_pid().kP(tuning_value);
plane.quadplane.attitude_control->get_rate_pitch_pid().kI(tuning_value);
break;
case TUNING_Q_RATE_PITCH_KP:
plane.quadplane.attitude_control->get_rate_pitch_pid().kP(tuning_value);
break;
case TUNING_Q_RATE_PITCH_KI:
plane.quadplane.attitude_control->get_rate_pitch_pid().kI(tuning_value);
break;
case TUNING_Q_RATE_PITCH_KD:
plane.quadplane.attitude_control->get_rate_pitch_pid().kD(tuning_value);
break;
case TUNING_Q_RATE_YAW_KPI:
plane.quadplane.attitude_control->get_rate_yaw_pid().kP(tuning_value);
plane.quadplane.attitude_control->get_rate_yaw_pid().kI(tuning_value);
break;
case TUNING_Q_RATE_YAW_KP:
plane.quadplane.attitude_control->get_rate_yaw_pid().kP(tuning_value);
break;
case TUNING_Q_RATE_YAW_KI:
plane.quadplane.attitude_control->get_rate_yaw_pid().kI(tuning_value);
break;
case TUNING_Q_RATE_YAW_KD:
plane.quadplane.attitude_control->get_rate_yaw_pid().kD(tuning_value);
break;
case TUNING_Q_ANG_ROLL_KP:
plane.quadplane.attitude_control->get_angle_roll_p().kP(tuning_value);
break;
case TUNING_Q_ANG_PITCH_KP:
plane.quadplane.attitude_control->get_angle_pitch_p().kP(tuning_value);
break;
case TUNING_Q_ANG_YAW_KP:
plane.quadplane.attitude_control->get_angle_yaw_p().kP(tuning_value);
break;
case TUNING_Q_PXY_P:
plane.quadplane.p_pos_xy.kP(tuning_value);
break;
case TUNING_Q_PZ_P:
plane.quadplane.p_alt_hold.kP(tuning_value);
break;
case TUNING_Q_VXY_P:
plane.quadplane.pi_vel_xy.kP(tuning_value);
break;
case TUNING_Q_VXY_I:
plane.quadplane.pi_vel_xy.kI(tuning_value);
break;
case TUNING_Q_VZ_P:
plane.quadplane.p_vel_z.kP(tuning_value);
break;
case TUNING_Q_AZ_P:
plane.quadplane.pid_accel_z.kP(tuning_value);
break;
case TUNING_Q_AZ_I:
plane.quadplane.pid_accel_z.kI(tuning_value);
break;
case TUNING_Q_AZ_D:
plane.quadplane.pid_accel_z.kD(tuning_value);
break;
default:
return;
}
Log_Write_Parameter_Tuning((uint8_t)parm.get(), tuning_value, minimum, maximum);
}
/*
log a tuning change
*/
void Tuning::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_low, float tune_high)
{
struct log_ParameterTuning pkt_tune = {
LOG_PACKET_HEADER_INIT(LOG_PARAMTUNE_MSG),
time_us : AP_HAL::micros64(),
parameter : param,
tuning_value : tuning_val,
tuning_low : tune_low,
tuning_high : tune_high
};
plane.DataFlash.WriteBlock(&pkt_tune, sizeof(pkt_tune));
}

75
ArduPlane/tuning.h
Unescape View File

@ -0,0 +1,75 @@ @@ -0,0 +1,75 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
Plane transmitter tuning
*/
class Tuning
{
public:
friend class Plane;
Tuning(void);
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
void check_input(void);
private:
AP_Int8 channel;
AP_Int8 parm;
AP_Float minimum;
AP_Float maximum;
uint8_t last_input_pct = 255;
uint32_t last_check_ms;
struct PACKED log_ParameterTuning {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t parameter; // parameter we are tuning, e.g. 39 is CH6_CIRCLE_RATE
float tuning_value; // normalized value used inside tuning() function
float tuning_low; // tuning low end value
float tuning_high; // tuning high end value
};
void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_low, float tune_high);
enum tuning_func {
TUNING_NONE = 0,
TUNING_Q_RATE_ROLL_PITCH_KPI = 1,
TUNING_Q_RATE_ROLL_PITCH_KP = 2,
TUNING_Q_RATE_ROLL_PITCH_KI = 3,
TUNING_Q_RATE_ROLL_PITCH_KD = 4,
TUNING_Q_RATE_ROLL_KPI = 5,
TUNING_Q_RATE_ROLL_KP = 6,
TUNING_Q_RATE_ROLL_KI = 7,
TUNING_Q_RATE_ROLL_KD = 8,
TUNING_Q_RATE_PITCH_KPI = 9,
TUNING_Q_RATE_PITCH_KP = 10,
TUNING_Q_RATE_PITCH_KI = 11,
TUNING_Q_RATE_PITCH_KD = 12,
TUNING_Q_RATE_YAW_KPI = 13,
TUNING_Q_RATE_YAW_KP = 14,
TUNING_Q_RATE_YAW_KI = 15,
TUNING_Q_RATE_YAW_KD = 16,
TUNING_Q_ANG_ROLL_KP = 17,
TUNING_Q_ANG_PITCH_KP = 18,
TUNING_Q_ANG_YAW_KP = 19,
TUNING_Q_PXY_P = 20,
TUNING_Q_PZ_P = 21,
TUNING_Q_VXY_P = 22,
TUNING_Q_VXY_I = 23,
TUNING_Q_VZ_P = 24,
TUNING_Q_AZ_P = 25,
TUNING_Q_AZ_I = 26,
TUNING_Q_AZ_D = 27,
};
};
Write Preview
Loading…
Cancel
Save