mirror of https://github.com/ArduPilot/ardupilot
Copter: System ID mode
This commit is contained in:
Leonard Hall
Randy Mackay
parent
91be844b66
commit
c11f2247eb
|
|
@ -43,6 +43,7 @@
|
|||
//#define MODE_RTL_ENABLED DISABLED // disable rtl mode support
|
||||
//#define MODE_SMARTRTL_ENABLED DISABLED // disable smartrtl mode support
|
||||
//#define MODE_SPORT_ENABLED DISABLED // disable sport mode support
|
||||
//#define MODE_SYSTEMID_ENABLED DISABLED // disable system ID mode support
|
||||
//#define MODE_THROW_ENABLED DISABLED // disable throw mode support
|
||||
//#define MODE_ZIGZAG_ENABLED DISABLED // disable zigzag mode support
|
||||
//#define OSD_ENABLED DISABLED // disable on-screen-display support
|
||||
|
|
|
|||
|
|
@ -227,6 +227,7 @@ public:
|
|||
friend class ModeSport;
|
||||
friend class ModeStabilize;
|
||||
friend class ModeStabilize_Heli;
|
||||
friend class ModeSystemId;
|
||||
friend class ModeThrow;
|
||||
friend class ModeZigZag;
|
||||
|
||||
|
|
@ -762,6 +763,8 @@ private:
|
|||
#endif
|
||||
void Log_Write_Precland();
|
||||
void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
|
||||
void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out);
|
||||
void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z);
|
||||
void Log_Write_Vehicle_Startup_Messages();
|
||||
void log_init(void);
|
||||
|
||||
|
|
@ -930,6 +933,9 @@ private:
|
|||
#if MODE_SPORT_ENABLED == ENABLED
|
||||
ModeSport mode_sport;
|
||||
#endif
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
ModeSystemId mode_systemid;
|
||||
#endif
|
||||
#if ADSB_ENABLED == ENABLED
|
||||
ModeAvoidADSB mode_avoid_adsb;
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -278,6 +278,74 @@ void Copter::Log_Sensor_Health()
|
|||
}
|
||||
}
|
||||
|
||||
struct PACKED log_SysIdD {
|
||||
LOG_PACKET_HEADER;
|
||||
uint64_t time_us;
|
||||
float waveform_time;
|
||||
float waveform_sample;
|
||||
float waveform_freq;
|
||||
float angle_x;
|
||||
float angle_y;
|
||||
float angle_z;
|
||||
float accel_x;
|
||||
float accel_y;
|
||||
float accel_z;
|
||||
};
|
||||
|
||||
// Write an rate packet
|
||||
void Copter::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z)
|
||||
{
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
struct log_SysIdD pkt_sidd = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_SYSIDD_MSG),
|
||||
time_us : AP_HAL::micros64(),
|
||||
waveform_time : waveform_time,
|
||||
waveform_sample : waveform_sample,
|
||||
waveform_freq : waveform_freq,
|
||||
angle_x : angle_x,
|
||||
angle_y : angle_y,
|
||||
angle_z : angle_z,
|
||||
accel_x : accel_x,
|
||||
accel_y : accel_y,
|
||||
accel_z : accel_z
|
||||
};
|
||||
logger.WriteBlock(&pkt_sidd, sizeof(pkt_sidd));
|
||||
#endif
|
||||
}
|
||||
|
||||
struct PACKED log_SysIdS {
|
||||
LOG_PACKET_HEADER;
|
||||
uint64_t time_us;
|
||||
uint8_t systemID_axis;
|
||||
float waveform_magnitude;
|
||||
float frequency_start;
|
||||
float frequency_stop;
|
||||
float time_fade_in;
|
||||
float time_const_freq;
|
||||
float time_record;
|
||||
float time_fade_out;
|
||||
};
|
||||
|
||||
// Write an rate packet
|
||||
void Copter::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out)
|
||||
{
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
struct log_SysIdS pkt_sids = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_SYSIDS_MSG),
|
||||
time_us : AP_HAL::micros64(),
|
||||
systemID_axis : systemID_axis,
|
||||
waveform_magnitude : waveform_magnitude,
|
||||
frequency_start : frequency_start,
|
||||
frequency_stop : frequency_stop,
|
||||
time_fade_in : time_fade_in,
|
||||
time_const_freq : time_const_freq,
|
||||
time_record : time_record,
|
||||
time_fade_out : time_fade_out
|
||||
};
|
||||
logger.WriteBlock(&pkt_sids, sizeof(pkt_sids));
|
||||
#endif
|
||||
}
|
||||
|
||||
struct PACKED log_Heli {
|
||||
LOG_PACKET_HEADER;
|
||||
uint64_t time_us;
|
||||
|
|
@ -416,6 +484,10 @@ const struct LogStructure Copter::log_structure[] = {
|
|||
{ LOG_PRECLAND_MSG, sizeof(log_Precland),
|
||||
"PL", "QBBfffffffIIB", "TimeUS,Heal,TAcq,pX,pY,vX,vY,mX,mY,mZ,LastMeasUS,EKFOutl,Est", "s--ddmmddms--","F--00BB00BC--" },
|
||||
#endif
|
||||
{ LOG_SYSIDD_MSG, sizeof(log_SysIdD),
|
||||
"SIDD", "Qfffffffff", "TimeUS,Time,Targ,F,Gx,Gy,Gz,Ax,Ay,Az", "ss-zkkkooo", "F---------" },
|
||||
{ LOG_SYSIDS_MSG, sizeof(log_SysIdS),
|
||||
"SIDS", "QBfffffff", "TimeUS,Ax,Mag,FSt,FSp,TFin,TC,TR,TFout", "s--ssssss", "F--------" },
|
||||
{ LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
|
||||
"GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-000000" },
|
||||
};
|
||||
|
|
@ -429,7 +501,6 @@ void Copter::Log_Write_Vehicle_Startup_Messages()
|
|||
gps.Write_AP_Logger_Log_Startup_messages();
|
||||
}
|
||||
|
||||
|
||||
void Copter::log_init(void)
|
||||
{
|
||||
logger.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||
|
|
@ -452,6 +523,8 @@ void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float t
|
|||
void Copter::Log_Sensor_Health() {}
|
||||
void Copter::Log_Write_Precland() {}
|
||||
void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}
|
||||
void Copter::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out) {}
|
||||
void Copter::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) {}
|
||||
void Copter::Log_Write_Vehicle_Startup_Messages() {}
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
|
|
|
|||
|
|
@ -262,42 +262,42 @@ const AP_Param::Info Copter::var_info[] = {
|
|||
// @Param: FLTMODE1
|
||||
// @DisplayName: Flight Mode 1
|
||||
// @Description: Flight mode when Channel 5 pwm is <= 1230
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode1, "FLTMODE1", (uint8_t)FLIGHT_MODE_1),
|
||||
|
||||
// @Param: FLTMODE2
|
||||
// @DisplayName: Flight Mode 2
|
||||
// @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode2, "FLTMODE2", (uint8_t)FLIGHT_MODE_2),
|
||||
|
||||
// @Param: FLTMODE3
|
||||
// @DisplayName: Flight Mode 3
|
||||
// @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode3, "FLTMODE3", (uint8_t)FLIGHT_MODE_3),
|
||||
|
||||
// @Param: FLTMODE4
|
||||
// @DisplayName: Flight Mode 4
|
||||
// @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode4, "FLTMODE4", (uint8_t)FLIGHT_MODE_4),
|
||||
|
||||
// @Param: FLTMODE5
|
||||
// @DisplayName: Flight Mode 5
|
||||
// @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode5, "FLTMODE5", (uint8_t)FLIGHT_MODE_5),
|
||||
|
||||
// @Param: FLTMODE6
|
||||
// @DisplayName: Flight Mode 6
|
||||
// @Description: Flight mode when Channel 5 pwm is >=1750
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode6, "FLTMODE6", (uint8_t)FLIGHT_MODE_6),
|
||||
|
||||
|
|
@ -934,6 +934,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
AP_SUBGROUPINFO(oa, "OA_", 33, ParametersG2, AP_OAPathPlanner),
|
||||
#endif
|
||||
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
// @Group: SID
|
||||
// @Path: mode_systemid.cpp
|
||||
AP_SUBGROUPPTR(mode_systemid_ptr, "SID", 34, ParametersG2, ModeSystemId),
|
||||
#endif
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
|
@ -1017,6 +1023,9 @@ ParametersG2::ParametersG2(void)
|
|||
#if AUTOTUNE_ENABLED == ENABLED
|
||||
,autotune_ptr(&copter.autotune)
|
||||
#endif
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
,mode_systemid_ptr(&copter.mode_systemid)
|
||||
#endif
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -604,6 +604,11 @@ public:
|
|||
// object avoidance path planning
|
||||
AP_OAPathPlanner oa;
|
||||
#endif
|
||||
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
// we need a pointer to the mode for the G2 table
|
||||
void *mode_systemid_ptr;
|
||||
#endif
|
||||
};
|
||||
|
||||
extern const AP_Param::Info var_info[];
|
||||
|
|
|
|||
|
|
@ -353,6 +353,12 @@
|
|||
# define MODE_SPORT_ENABLED !HAL_MINIMIZE_FEATURES
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// System ID - conduct system identification tests on vehicle
|
||||
#ifndef MODE_SYSTEMID_ENABLED
|
||||
# define MODE_SYSTEMID_ENABLED !HAL_MINIMIZE_FEATURES
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Throw - fly vehicle after throwing it in the air
|
||||
#ifndef MODE_THROW_ENABLED
|
||||
|
|
|
|||
|
|
@ -94,7 +94,8 @@ enum tuning_func {
|
|||
TUNING_RATE_YAW_FF = 54, // body frame yaw rate controller FF term
|
||||
TUNING_RATE_MOT_YAW_HEADROOM = 55, // motors yaw headroom minimum
|
||||
TUNING_RATE_YAW_FILT = 56, // yaw rate input filter
|
||||
TUNING_WINCH = 57 // winch control (not actually a value to be tuned)
|
||||
TUNING_WINCH = 57, // winch control (not actually a value to be tuned)
|
||||
SYSTEM_ID_MAGNITUDE = 58 // magnitude of the system ID signal
|
||||
};
|
||||
|
||||
// Acro Trainer types
|
||||
|
|
@ -189,6 +190,8 @@ enum LoggingParameters {
|
|||
LOG_HELI_MSG,
|
||||
LOG_PRECLAND_MSG,
|
||||
LOG_GUIDEDTARGET_MSG,
|
||||
LOG_SYSIDD_MSG,
|
||||
LOG_SYSIDS_MSG,
|
||||
};
|
||||
|
||||
#define MASK_LOG_ATTITUDE_FAST (1<<0)
|
||||
|
|
|
|||
|
|
@ -159,6 +159,12 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode)
|
|||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
case Mode::Number::SYSTEMID:
|
||||
ret = (Mode *)g2.mode_systemid_ptr;
|
||||
break;
|
||||
#endif
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -35,6 +35,7 @@ public:
|
|||
FLOWHOLD = 22, // FLOWHOLD holds position with optical flow without rangefinder
|
||||
FOLLOW = 23, // follow attempts to follow another vehicle or ground station
|
||||
ZIGZAG = 24, // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B
|
||||
SYSTEMID = 25, // System ID mode produces automated system identification signals in the controllers
|
||||
};
|
||||
|
||||
// constructor
|
||||
|
|
@ -1172,6 +1173,72 @@ private:
|
|||
};
|
||||
#endif
|
||||
|
||||
class ModeSystemId : public Mode {
|
||||
|
||||
public:
|
||||
ModeSystemId(void);
|
||||
|
||||
bool init(bool ignore_checks) override;
|
||||
void run() override;
|
||||
|
||||
bool requires_GPS() const override { return true; }
|
||||
bool has_manual_throttle() const override { return false; }
|
||||
bool allows_arming(bool from_gcs) const override { return true; };
|
||||
bool is_autopilot() const override { return false; }
|
||||
bool stop_attitude_logging() const override { return true; }
|
||||
void set_magnitude(float input) {waveform_magnitude = input;}
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
protected:
|
||||
|
||||
const char *name() const override { return "SYSTEMID"; }
|
||||
const char *name4() const override { return "SYSI"; }
|
||||
|
||||
private:
|
||||
void log_data();
|
||||
float waveform(float time);
|
||||
|
||||
enum AxisType {
|
||||
NONE = 0, // none
|
||||
INPUT_ROLL = 1, // angle input roll axis is being excited
|
||||
INPUT_PITCH = 2, // angle pitch axis is being excited
|
||||
INPUT_YAW = 3, // angle yaw axis is being excited
|
||||
RECOVER_ROLL = 4, // angle roll axis is being excited
|
||||
RECOVER_PITCH = 5, // angle pitch axis is being excited
|
||||
RECOVER_YAW = 6, // angle yaw axis is being excited
|
||||
RATE_ROLL = 7, // rate roll axis is being excited
|
||||
RATE_PITCH = 8, // rate pitch axis is being excited
|
||||
RATE_YAW = 9, // rate yaw axis is being excited
|
||||
MIX_ROLL = 10, // mixer roll axis is being excited
|
||||
MIX_PITCH = 11, // mixer pitch axis is being excited
|
||||
MIX_YAW = 12, // mixer pitch axis is being excited
|
||||
MIX_THROTTLE = 13, // mixer throttle axis is being excited
|
||||
};
|
||||
|
||||
AP_Int8 systemID_axis; // Controls which axis are being excited
|
||||
AP_Float waveform_magnitude; // Magnitude of chirp waveform
|
||||
AP_Float frequency_start; // Frequency at the start of the chirp
|
||||
AP_Float frequency_stop; // Frequency at the end of the chirp
|
||||
AP_Float time_fade_in; // Time to reach maximum amplitude of chirp
|
||||
AP_Float time_record; // Time taken to complete the chirp waveform
|
||||
AP_Float time_fade_out; // Time to reach zero amplitude after chirp finishes
|
||||
|
||||
bool att_bf_feedforward; // Setting of attitude_control->get_bf_feedforward
|
||||
float waveform_time; // Time reference for waveform
|
||||
float waveform_sample; // Current waveform sample
|
||||
float waveform_freq_rads; // Instantaneous waveform frequency
|
||||
float time_const_freq; // Time at constant frequency before chirp starts
|
||||
int8_t log_subsample; // Subsample multiple for logging.
|
||||
|
||||
// System ID states
|
||||
enum SystemIDModeState {
|
||||
SystemID_Stopped,
|
||||
SystemID_Testing
|
||||
};
|
||||
|
||||
SystemIDModeState systemIDState;
|
||||
};
|
||||
|
||||
class ModeThrow : public Mode {
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,333 @@
|
|||
#include "Copter.h"
|
||||
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
|
||||
/*
|
||||
* Init and run calls for systemId, flight mode
|
||||
*/
|
||||
|
||||
const AP_Param::GroupInfo ModeSystemId::var_info[] = {
|
||||
|
||||
// @Param: _AXIS
|
||||
// @DisplayName: System identification axis
|
||||
// @Description: Controls which axis are being excited
|
||||
// @User: Standard
|
||||
// @Values: 0:None, 1:Input Roll Angle, 2:Input Pitch Angle, 3:Input Yaw Angle, 4:Recovery Roll Angle, 5:Recovery Pitch Angle, 6:Recovery Yaw Angle, 7:Rate Roll, 8:Rate Pitch, 9:Rate Yaw, 10:Mixer Roll, 11:Mixer Pitch, 12:Mixer Yaw, 13:Mixer Thrust
|
||||
AP_GROUPINFO("_AXIS", 1, ModeSystemId, systemID_axis, 0),
|
||||
|
||||
// @Param: _MAG
|
||||
// @DisplayName: Chirp Magnitude
|
||||
// @Description: Magnitude of sweep in deg, deg/s and 0-1 for mixer outputs.
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("_MAG", 2, ModeSystemId, waveform_magnitude, 15),
|
||||
|
||||
// @Param: _F_START_HZ
|
||||
// @DisplayName: Start Frequency
|
||||
// @Description: Frequency at the start of the sweep
|
||||
// @Range: 0.01 100
|
||||
// @Units: Hz
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("_F_START_HZ", 3, ModeSystemId, frequency_start, 0.5f),
|
||||
|
||||
// @Param: _F_STOP_HZ
|
||||
// @DisplayName: Stop Frequency
|
||||
// @Description: Frequency at the end of the sweep
|
||||
// @Range: 0.01 100
|
||||
// @Units: Hz
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("_F_STOP_HZ", 4, ModeSystemId, frequency_stop, 40),
|
||||
|
||||
// @Param: _T_FADE_IN
|
||||
// @DisplayName: Fade in time
|
||||
// @Description: Time to reach maximum amplitude of sweep
|
||||
// @Range: 0 20
|
||||
// @Units: s
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("_T_FADE_IN", 5, ModeSystemId, time_fade_in, 15),
|
||||
|
||||
// @Param: _T_REC
|
||||
// @DisplayName: Total Sweep length
|
||||
// @Description: Time taken to complete the sweep
|
||||
// @Range: 0 255
|
||||
// @Units: s
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("_T_REC", 6, ModeSystemId, time_record, 70),
|
||||
|
||||
// @Param: _T_FADE_OUT
|
||||
// @DisplayName: Fade out time
|
||||
// @Description: Time to reach zero amplitude at the end of the sweep
|
||||
// @Range: 0 5
|
||||
// @Units: s
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("_T_FADE_OUT", 7, ModeSystemId, time_fade_out, 2),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
ModeSystemId::ModeSystemId(void) : Mode()
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
||||
#define SYSTEM_ID_DELAY 1.0f // speed below which it is always safe to switch to loiter
|
||||
|
||||
// systemId_init - initialise systemId controller
|
||||
bool ModeSystemId::init(bool ignore_checks)
|
||||
{
|
||||
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
|
||||
if (motors->armed() && copter.ap.land_complete && !copter.flightmode->has_manual_throttle()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
copter.input_manager.set_use_stab_col(true);
|
||||
#endif
|
||||
|
||||
att_bf_feedforward = attitude_control->get_bf_feedforward();
|
||||
waveform_time = 0.0f;
|
||||
time_const_freq = 2.0f / frequency_start; // Two full cycles at the starting frequency
|
||||
systemIDState = SystemID_Testing;
|
||||
log_subsample = 0;
|
||||
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "SystemID Starting: axis=%d", (unsigned)systemID_axis);
|
||||
|
||||
copter.Log_Write_SysID_Setup(systemID_axis, waveform_magnitude, frequency_start, frequency_stop, time_fade_in, time_const_freq, time_record, time_fade_out);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// systemId_run - runs the systemId controller
|
||||
// should be called at 100hz or more
|
||||
void ModeSystemId::run()
|
||||
{
|
||||
// apply simple mode transform to pilot inputs
|
||||
update_simple_mode();
|
||||
|
||||
// convert pilot input to lean angles
|
||||
float target_roll, target_pitch;
|
||||
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
|
||||
|
||||
// get pilot's desired yaw rate
|
||||
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
||||
|
||||
if (!motors->armed()) {
|
||||
// Motors should be Stopped
|
||||
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
|
||||
// Tradheli doesn't set spool state to ground idle when throttle stick is zero. Ground idle only set when
|
||||
// motor interlock is disabled.
|
||||
} else if (copter.ap.throttle_zero && !copter.is_tradheli()) {
|
||||
// Attempting to Land
|
||||
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
|
||||
} else {
|
||||
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
||||
}
|
||||
|
||||
switch (motors->get_spool_state()) {
|
||||
case AP_Motors::SpoolState::SHUT_DOWN:
|
||||
// Motors Stopped
|
||||
attitude_control->set_yaw_target_to_current_heading();
|
||||
attitude_control->reset_rate_controller_I_terms();
|
||||
break;
|
||||
case AP_Motors::SpoolState::GROUND_IDLE:
|
||||
// Landed
|
||||
// Tradheli initializes targets when going from disarmed to armed state.
|
||||
// init_targets_on_arming is always set true for multicopter.
|
||||
if (motors->init_targets_on_arming()) {
|
||||
attitude_control->set_yaw_target_to_current_heading();
|
||||
attitude_control->reset_rate_controller_I_terms();
|
||||
}
|
||||
break;
|
||||
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
|
||||
// clear landing flag above zero throttle
|
||||
if (!motors->limit.throttle_lower) {
|
||||
set_land_complete(false);
|
||||
}
|
||||
break;
|
||||
case AP_Motors::SpoolState::SPOOLING_UP:
|
||||
case AP_Motors::SpoolState::SPOOLING_DOWN:
|
||||
// do nothing
|
||||
break;
|
||||
}
|
||||
|
||||
// get pilot's desired throttle
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
float pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
|
||||
#else
|
||||
float pilot_throttle_scaled = get_pilot_desired_throttle();
|
||||
#endif
|
||||
|
||||
if((systemIDState == SystemID_Testing) && (!is_positive(frequency_start) || !is_positive(frequency_stop) || is_negative(time_fade_in) || !is_positive(time_record) || is_negative(time_fade_out) || (time_record <= time_const_freq))) {
|
||||
systemIDState = SystemID_Stopped;
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "SystemID Parameter Error");
|
||||
}
|
||||
|
||||
waveform_time += G_Dt;
|
||||
waveform_sample = waveform(waveform_time - SYSTEM_ID_DELAY);
|
||||
|
||||
switch (systemIDState) {
|
||||
case SystemID_Stopped:
|
||||
break;
|
||||
case SystemID_Testing:
|
||||
attitude_control->bf_feedforward(att_bf_feedforward);
|
||||
|
||||
if(copter.ap.land_complete) {
|
||||
systemIDState = SystemID_Stopped;
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "SystemID Stopped: Landed");
|
||||
break;
|
||||
}
|
||||
if(attitude_control->lean_angle()*100 > attitude_control->lean_angle_max()) {
|
||||
systemIDState = SystemID_Stopped;
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "SystemID Stopped: lean=%f max=%f", (double)attitude_control->lean_angle(), (double)attitude_control->lean_angle_max());
|
||||
break;
|
||||
}
|
||||
if(waveform_time > SYSTEM_ID_DELAY + time_fade_in + time_const_freq + time_record + time_fade_out) {
|
||||
systemIDState = SystemID_Stopped;
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "SystemID Finished");
|
||||
break;
|
||||
}
|
||||
|
||||
switch (systemID_axis) {
|
||||
case NONE:
|
||||
systemIDState = SystemID_Stopped;
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "SystemID Stopped: axis = 0");
|
||||
break;
|
||||
case INPUT_ROLL:
|
||||
target_roll += waveform_sample*100.0f;
|
||||
break;
|
||||
case INPUT_PITCH:
|
||||
target_pitch += waveform_sample*100.0f;
|
||||
break;
|
||||
case INPUT_YAW:
|
||||
target_yaw_rate += waveform_sample*100.0f;
|
||||
break;
|
||||
case RECOVER_ROLL:
|
||||
target_roll += waveform_sample*100.0f;
|
||||
attitude_control->bf_feedforward(false);
|
||||
break;
|
||||
case RECOVER_PITCH:
|
||||
target_pitch += waveform_sample*100.0f;
|
||||
attitude_control->bf_feedforward(false);
|
||||
break;
|
||||
case RECOVER_YAW:
|
||||
target_yaw_rate += waveform_sample*100.0f;
|
||||
attitude_control->bf_feedforward(false);
|
||||
break;
|
||||
case RATE_ROLL:
|
||||
attitude_control->rate_bf_roll_sysid(radians(waveform_sample));
|
||||
break;
|
||||
case RATE_PITCH:
|
||||
attitude_control->rate_bf_pitch_sysid(radians(waveform_sample));
|
||||
break;
|
||||
case RATE_YAW:
|
||||
attitude_control->rate_bf_yaw_sysid(radians(waveform_sample));
|
||||
break;
|
||||
case MIX_ROLL:
|
||||
attitude_control->actuator_roll_sysid(waveform_sample);
|
||||
break;
|
||||
case MIX_PITCH:
|
||||
attitude_control->actuator_pitch_sysid(waveform_sample);
|
||||
break;
|
||||
case MIX_YAW:
|
||||
attitude_control->actuator_yaw_sysid(waveform_sample);
|
||||
break;
|
||||
case MIX_THROTTLE:
|
||||
pilot_throttle_scaled += waveform_sample;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
// call attitude controller
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
|
||||
|
||||
// body-frame rate controller is run directly from 100hz loop
|
||||
|
||||
// output pilot's throttle
|
||||
if (copter.is_tradheli()) {
|
||||
attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
|
||||
} else {
|
||||
attitude_control->set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt);
|
||||
}
|
||||
|
||||
if(log_subsample <= 0){
|
||||
log_data();
|
||||
if(copter.should_log(MASK_LOG_ATTITUDE_FAST) && copter.should_log(MASK_LOG_ATTITUDE_MED)) {
|
||||
log_subsample = 1;
|
||||
} else if(copter.should_log(MASK_LOG_ATTITUDE_FAST)){
|
||||
log_subsample = 2;
|
||||
} else if(copter.should_log(MASK_LOG_ATTITUDE_MED)){
|
||||
log_subsample = 4;
|
||||
} else {
|
||||
log_subsample = 8;
|
||||
}
|
||||
}
|
||||
log_subsample -= 1;
|
||||
}
|
||||
|
||||
// init_test - initialises the test
|
||||
void ModeSystemId::log_data()
|
||||
{
|
||||
int8_t index = copter.ahrs.get_primary_gyro_index();
|
||||
if(index < 0){
|
||||
index = 0;
|
||||
}
|
||||
Vector3f delta_angle;
|
||||
copter.ins.get_delta_angle(index, delta_angle);
|
||||
float delta_angle_dt = copter.ins.get_delta_angle_dt(index);
|
||||
|
||||
index = copter.ahrs.get_primary_accel_index();
|
||||
if(index < 0){
|
||||
index = 0;
|
||||
}
|
||||
Vector3f delta_velocity;
|
||||
copter.ins.get_delta_velocity(index, delta_velocity);
|
||||
float delta_velocity_dt = copter.ins.get_delta_velocity_dt(index);
|
||||
|
||||
if(is_positive(delta_angle_dt) && is_positive(delta_velocity_dt)){
|
||||
copter.Log_Write_SysID_Data(waveform_time, waveform_sample, waveform_freq_rads / (2 * M_PI), degrees(delta_angle.x / delta_angle_dt), degrees(delta_angle.y / delta_angle_dt), degrees(delta_angle.z / delta_angle_dt), delta_velocity.x / delta_velocity_dt, delta_velocity.y / delta_velocity_dt, delta_velocity.z / delta_velocity_dt);
|
||||
}
|
||||
|
||||
// Full rate logging of attitude, rate and pid loops
|
||||
copter.Log_Write_Attitude();
|
||||
}
|
||||
|
||||
// init_test - initialises the test
|
||||
float ModeSystemId::waveform(float time)
|
||||
{
|
||||
float wMin = 2 * M_PI * frequency_start;
|
||||
float wMax = 2 * M_PI * frequency_stop;
|
||||
|
||||
float window;
|
||||
float output;
|
||||
|
||||
float B = logf(wMax / wMin);
|
||||
|
||||
if(time <= 0.0f) {
|
||||
window = 0.0f;
|
||||
} else if (time <= time_fade_in) {
|
||||
window = 0.5 - 0.5 * cosf(M_PI * time / time_fade_in);
|
||||
} else if (time <= time_record - time_fade_out) {
|
||||
window = 1.0;
|
||||
} else if (time <= time_record) {
|
||||
window = 0.5 - 0.5 * cosf(M_PI * (time - (time_record - time_fade_out)) / time_fade_out + M_PI);
|
||||
} else {
|
||||
window = 0.0;
|
||||
}
|
||||
|
||||
if(time <= 0.0f) {
|
||||
waveform_freq_rads = wMin;
|
||||
output = 0.0f;
|
||||
} else if (time <= time_const_freq) {
|
||||
waveform_freq_rads = wMin;
|
||||
output = window * waveform_magnitude * sinf(wMin * time - wMin * time_const_freq);
|
||||
} else if (time <= time_record) {
|
||||
waveform_freq_rads = wMin * expf(B * (time - time_const_freq) / (time_record - time_const_freq));
|
||||
output = window * waveform_magnitude * sinf((wMin * (time_record - time_const_freq) / B) * (expf(B * (time - time_const_freq) / (time_record - time_const_freq)) - 1));
|
||||
} else {
|
||||
waveform_freq_rads = wMax;
|
||||
output = 0.0f;
|
||||
}
|
||||
return output;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -227,5 +227,11 @@ void Copter::tuning()
|
|||
g2.winch.set_desired_rate(tuning_value);
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if MODE_SYSTEMID_ENABLED == ENABLED
|
||||
case SYSTEM_ID_MAGNITUDE:
|
||||
copter.mode_systemid.set_magnitude(tuning_value);
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue