aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: added QTUN logging for quadplane

Browse Source
This commit is contained in:
Andrew Tridgell 2016-03-25 12:33:19 +11:00
parent 0a199945a2
commit 4d7beab8cc
4 changed files with 36 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/Log.cpp
View File

@ -500,6 +500,8 @@ static const struct LogStructure log_structure[] = {
"ATRP", "QBBcfff", "TimeUS,Type,State,Servo,Demanded,Achieved,P" },
{ LOG_STATUS_MSG, sizeof(log_Status),
"STAT", "QBfBBBBBB", "TimeUS,isFlying,isFlyProb,Armed,Safety,Crash,Still,Stage,Hit" },
{ LOG_QTUN_MSG, sizeof(QuadPlane::log_QControl_Tuning),
"QTUN", "Qhfffehh", "TimeUS,AngBst,ThrOut,DAlt,Alt,BarAlt,DCRt,CRt" },
#if OPTFLOW == ENABLED
{ LOG_OPTFLOW_MSG, sizeof(log_Optflow),
"OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY" },

3
ArduPlane/defines.h
View File

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

18
ArduPlane/quadplane.cpp
View File

@ -915,6 +915,7 @@ void QuadPlane::motors_output(void)
{
motors->output();
plane.DataFlash.Log_Write_Rate(plane.ahrs, *motors, *attitude_control, *pos_control);
Log_Write_QControl_Tuning();
}
/*
@ -1301,3 +1302,20 @@ bool QuadPlane::verify_vtol_land(const AP_Mission::Mission_Command &cmd)
check_land_complete();
return false;
}
// Write a control tuning packet
void QuadPlane::Log_Write_QControl_Tuning()
{
struct log_QControl_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_QTUN_MSG),
time_us : AP_HAL::micros64(),
angle_boost : attitude_control->angle_boost(),
throttle_out : motors->get_throttle(),
desired_alt : pos_control->get_alt_target() / 100.0f,
inav_alt : inertial_nav.get_altitude() / 100.0f,
baro_alt : (int32_t)plane.barometer.get_altitude() * 100,
desired_climb_rate : (int16_t)pos_control->get_vel_target_z(),
climb_rate : (int16_t)inertial_nav.get_velocity_z()
};
plane.DataFlash.WriteBlock(&pkt, sizeof(pkt));
}

15
ArduPlane/quadplane.h
View File

@ -56,7 +56,19 @@ public:
uint8_t throttle_percentage(void) const {
return last_throttle * 0.1f;
}
struct PACKED log_QControl_Tuning {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t angle_boost;
float throttle_out;
float desired_alt;
float inav_alt;
int32_t baro_alt;
int16_t desired_climb_rate;
int16_t climb_rate;
};
private:
AP_AHRS_NavEKF &ahrs;
AP_Vehicle::MultiCopter aparm;
@ -129,6 +141,7 @@ private:
bool should_relax(void);
void motors_output(void);
void Log_Write_QControl_Tuning();
// setup correct aux channels for frame class
void setup_default_channels(uint8_t num_motors);