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

Plane: move sending of send_pid_tuning up

Browse Source
This commit is contained in:
Peter Barker 2019-03-01 10:33:00 +11:00 committed by Andrew Tridgell
parent eed73749f9
commit 94899bd2ed
3 changed files with 31 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
ArduPlane/GCS_Mavlink.cpp
View File

@ -324,7 +324,7 @@ void NOINLINE Plane::send_rpm(mavlink_channel_t chan)
} }
// sends a single pid info over the provided channel // sends a single pid info over the provided channel
void Plane::send_pid_info(const mavlink_channel_t chan, const AP_Logger::PID_Info *pid_info, void GCS_MAVLINK_Plane::send_pid_info(const AP_Logger::PID_Info *pid_info,
const uint8_t axis, const float achieved) const uint8_t axis, const float achieved)
{ {
if (pid_info == nullptr) { if (pid_info == nullptr) {
@ -345,44 +345,52 @@ void Plane::send_pid_info(const mavlink_channel_t chan, const AP_Logger::PID_Inf
/* /*
send PID tuning message send PID tuning message
*/ */
void Plane::send_pid_tuning(mavlink_channel_t chan) void GCS_MAVLINK_Plane::send_pid_tuning()
{ {
if (plane.control_mode == MANUAL) {
// no PIDs should be used in manual
return;
}
const Parameters &g = plane.g;
AP_AHRS &ahrs = AP::ahrs();
const Vector3f &gyro = ahrs.get_gyro(); const Vector3f &gyro = ahrs.get_gyro();
const AP_Logger::PID_Info *pid_info; const AP_Logger::PID_Info *pid_info;
if (g.gcs_pid_mask & TUNING_BITS_ROLL) { if (g.gcs_pid_mask & TUNING_BITS_ROLL) {
if (quadplane.in_vtol_mode()) { if (plane.quadplane.in_vtol_mode()) {
pid_info = &quadplane.attitude_control->get_rate_roll_pid().get_pid_info(); pid_info = &plane.quadplane.attitude_control->get_rate_roll_pid().get_pid_info();
} else { } else {
pid_info = &rollController.get_pid_info(); pid_info = &plane.rollController.get_pid_info();
} }
send_pid_info(chan, pid_info, PID_TUNING_ROLL, degrees(gyro.x)); send_pid_info(pid_info, PID_TUNING_ROLL, degrees(gyro.x));
} }
if (g.gcs_pid_mask & TUNING_BITS_PITCH) { if (g.gcs_pid_mask & TUNING_BITS_PITCH) {
if (quadplane.in_vtol_mode()) { if (plane.quadplane.in_vtol_mode()) {
pid_info = &quadplane.attitude_control->get_rate_pitch_pid().get_pid_info(); pid_info = &plane.quadplane.attitude_control->get_rate_pitch_pid().get_pid_info();
} else { } else {
pid_info = &pitchController.get_pid_info(); pid_info = &plane.pitchController.get_pid_info();
} }
send_pid_info(chan, pid_info, PID_TUNING_PITCH, degrees(gyro.y)); send_pid_info(pid_info, PID_TUNING_PITCH, degrees(gyro.y));
} }
if (g.gcs_pid_mask & TUNING_BITS_YAW) { if (g.gcs_pid_mask & TUNING_BITS_YAW) {
if (quadplane.in_vtol_mode()) { if (plane.quadplane.in_vtol_mode()) {
pid_info = &quadplane.attitude_control->get_rate_yaw_pid().get_pid_info(); pid_info = &plane.quadplane.attitude_control->get_rate_yaw_pid().get_pid_info();
} else { } else {
pid_info = &yawController.get_pid_info(); pid_info = &plane.yawController.get_pid_info();
} }
send_pid_info(chan, pid_info, PID_TUNING_YAW, degrees(gyro.z)); send_pid_info(pid_info, PID_TUNING_YAW, degrees(gyro.z));
} }
if (g.gcs_pid_mask & TUNING_BITS_STEER) { if (g.gcs_pid_mask & TUNING_BITS_STEER) {
send_pid_info(chan, &steerController.get_pid_info(), PID_TUNING_STEER, degrees(gyro.z)); send_pid_info(&plane.steerController.get_pid_info(), PID_TUNING_STEER, degrees(gyro.z));
} }
if ((g.gcs_pid_mask & TUNING_BITS_LAND) && (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND)) { if ((g.gcs_pid_mask & TUNING_BITS_LAND) && (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND)) {
send_pid_info(chan, landing.get_pid_info(), PID_TUNING_LANDING, degrees(gyro.z)); send_pid_info(plane.landing.get_pid_info(), PID_TUNING_LANDING, degrees(gyro.z));
} }
if (g.gcs_pid_mask & TUNING_BITS_ACCZ && quadplane.in_vtol_mode()) { if (g.gcs_pid_mask & TUNING_BITS_ACCZ && plane.quadplane.in_vtol_mode()) {
const Vector3f &accel = ahrs.get_accel_ef(); const Vector3f &accel = ahrs.get_accel_ef();
pid_info = &quadplane.pos_control->get_accel_z_pid().get_pid_info(); pid_info = &plane.quadplane.pos_control->get_accel_z_pid().get_pid_info();
send_pid_info(chan, pid_info, PID_TUNING_ACCZ, -accel.z); send_pid_info(pid_info, PID_TUNING_ACCZ, -accel.z);
} }
} }
@ -443,13 +451,6 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
plane.send_wind(chan); plane.send_wind(chan);
break; break;
case MSG_PID_TUNING:
if (plane.control_mode != MANUAL) {
CHECK_PAYLOAD_SIZE(PID_TUNING);
plane.send_pid_tuning(chan);
}
break;
case MSG_RPM: case MSG_RPM:
CHECK_PAYLOAD_SIZE(RPM); CHECK_PAYLOAD_SIZE(RPM);
plane.send_rpm(chan); plane.send_rpm(chan);

3
ArduPlane/GCS_Mavlink.h
View File

@ -42,9 +42,12 @@ protected:
uint64_t capabilities() const override; uint64_t capabilities() const override;
void send_nav_controller_output() const override; void send_nav_controller_output() const override;
void send_pid_tuning() override;
private: private:
void send_pid_info(const AP_Logger::PID_Info *pid_info, const uint8_t axis, const float achieved);
void handleMessage(mavlink_message_t * msg) override; void handleMessage(mavlink_message_t * msg) override;
bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; bool handle_guided_request(AP_Mission::Mission_Command &cmd) override;
void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override;

2
ArduPlane/Plane.h
View File

@ -792,8 +792,6 @@ private:
void send_fence_status(mavlink_channel_t chan); void send_fence_status(mavlink_channel_t chan);
void send_servo_out(mavlink_channel_t chan); void send_servo_out(mavlink_channel_t chan);
void send_wind(mavlink_channel_t chan); void send_wind(mavlink_channel_t chan);
void send_pid_info(const mavlink_channel_t chan, const AP_Logger::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); void send_rpm(mavlink_channel_t chan);
void send_aoa_ssa(mavlink_channel_t chan); void send_aoa_ssa(mavlink_channel_t chan);