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

Plane: Consolidate some of the PID logging details

Browse Source
This commit is contained in:
Michael du Breuil 2018-03-02 17:46:46 -07:00 committed by Tom Pittenger
parent b6c0e65dee
commit faea5d285c
3 changed files with 41 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
ArduPlane/GCS_Mavlink.cpp
View File

@ -317,6 +317,25 @@ void NOINLINE Plane::send_rpm(mavlink_channel_t chan)
}
}
// sends a single pid info over the provided channel
void Plane::send_pid_info(const mavlink_channel_t chan, const DataFlash_Class::PID_Info *pid_info,
const uint8_t axis, const float achieved)
{
if (pid_info == nullptr) {
return;
}
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
return;
}
mavlink_msg_pid_tuning_send(chan, axis,
pid_info->desired,
achieved,
pid_info->FF,
pid_info->P,
pid_info->I,
pid_info->D);
}
/*
send PID tuning message
*/
@ -324,84 +343,35 @@ void Plane::send_pid_tuning(mavlink_channel_t chan)
{
const Vector3f &gyro = ahrs.get_gyro();
const DataFlash_Class::PID_Info *pid_info;
if (g.gcs_pid_mask & 1) {
if (g.gcs_pid_mask & TUNING_BITS_ROLL) {
if (quadplane.in_vtol_mode()) {
pid_info = &quadplane.attitude_control->get_rate_roll_pid().get_pid_info();
} else {
pid_info = &rollController.get_pid_info();
}
mavlink_msg_pid_tuning_send(chan, PID_TUNING_ROLL,
pid_info->desired,
degrees(gyro.x),
pid_info->FF,
pid_info->P,
pid_info->I,
pid_info->D);
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
return;
}
send_pid_info(chan, pid_info, PID_TUNING_ROLL, degrees(gyro.x));
}
if (g.gcs_pid_mask & 2) {
if (g.gcs_pid_mask & TUNING_BITS_PITCH) {
if (quadplane.in_vtol_mode()) {
pid_info = &quadplane.attitude_control->get_rate_pitch_pid().get_pid_info();
} else {
pid_info = &pitchController.get_pid_info();
}
mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH,
pid_info->desired,
degrees(gyro.y),
pid_info->FF,
pid_info->P,
pid_info->I,
pid_info->D);
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
return;
}
send_pid_info(chan, pid_info, PID_TUNING_PITCH, degrees(gyro.y));
}
if (g.gcs_pid_mask & 4) {
if (g.gcs_pid_mask & TUNING_BITS_YAW) {
if (quadplane.in_vtol_mode()) {
pid_info = &quadplane.attitude_control->get_rate_yaw_pid().get_pid_info();
} else {
pid_info = &yawController.get_pid_info();
}
mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW,
pid_info->desired,
degrees(gyro.z),
pid_info->FF,
pid_info->P,
pid_info->I,
pid_info->D);
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
return;
}
send_pid_info(chan, pid_info, PID_TUNING_YAW, degrees(gyro.z));
}
if (g.gcs_pid_mask & 8) {
pid_info = &steerController.get_pid_info();
mavlink_msg_pid_tuning_send(chan, PID_TUNING_STEER,
pid_info->desired,
degrees(gyro.z),
pid_info->FF,
pid_info->P,
pid_info->I,
pid_info->D);
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
return;
}
if (g.gcs_pid_mask & TUNING_BITS_STEER) {
send_pid_info(chan, &steerController.get_pid_info(), PID_TUNING_STEER, degrees(gyro.z));
}
if ((g.gcs_pid_mask & 0x10) && (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND)) {
pid_info = landing.get_pid_info();
if (pid_info != nullptr) {
mavlink_msg_pid_tuning_send(chan, PID_TUNING_LANDING,
pid_info->desired,
gyro.z,
pid_info->FF,
pid_info->P,
pid_info->I,
pid_info->D);
}
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
return;
}
if ((g.gcs_pid_mask & TUNING_BITS_LAND) && (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND)) {
send_pid_info(chan, landing.get_pid_info(), PID_TUNING_LANDING, degrees(gyro.z));
}
}

1
ArduPlane/Plane.h
View File

@ -792,6 +792,7 @@ private:
void send_vfr_hud(mavlink_channel_t chan);
void send_simstate(mavlink_channel_t chan);
void send_wind(mavlink_channel_t chan);
void send_pid_info(const mavlink_channel_t chan, const DataFlash_Class::PID_Info *pid_info, const uint8_t axis, const float achieved);
void send_pid_tuning(mavlink_channel_t chan);
void send_rpm(mavlink_channel_t chan);

11
ArduPlane/defines.h
View File

@ -116,6 +116,17 @@ typedef enum GeofenceEnableReason {
GCS_TOGGLED //Fence enabled/disabled by the GCS via Mavlink
} GeofenceEnableReason;
// PID broadcast bitmask
enum tuning_pid_bits {
TUNING_BITS_ROLL = (1 << 0),
TUNING_BITS_PITCH = (1 << 1),
TUNING_BITS_YAW = (1 << 2),
TUNING_BITS_STEER = (1 << 3),
TUNING_BITS_LAND = (1 << 4),
TUNING_BITS_END // dummy just used for static checking
};
static_assert(TUNING_BITS_END <= (1 << 24) + 1, "Tuning bit mask is to large to be set by MAVLink");
// Logging message types
enum log_messages {