Browse Source

Rover: move sending of send_pid_tuning up

master
Peter Barker 6 years ago committed by Andrew Tridgell
parent
commit
1debd88083
  1. 12
      APMrover2/GCS_Mavlink.cpp
  2. 1
      APMrover2/GCS_Mavlink.h
  3. 1
      APMrover2/Rover.h

12
APMrover2/GCS_Mavlink.cpp

@ -171,9 +171,14 @@ void Rover::send_rpm(mavlink_channel_t chan) @@ -171,9 +171,14 @@ void Rover::send_rpm(mavlink_channel_t chan)
/*
send PID tuning message
*/
void Rover::send_pid_tuning(mavlink_channel_t chan)
void GCS_MAVLINK_Rover::send_pid_tuning()
{
Parameters &g = rover.g;
ParametersG2 &g2 = rover.g2;
const AP_AHRS &ahrs = AP::ahrs();
const AP_Logger::PID_Info *pid_info;
// steering PID
if (g.gcs_pid_mask & 1) {
pid_info = &g2.attitude_control.get_steering_rate_pid().get_pid_info();
@ -333,11 +338,6 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id) @@ -333,11 +338,6 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
rover.g2.windvane.send_wind(chan);
break;
case MSG_PID_TUNING:
CHECK_PAYLOAD_SIZE(PID_TUNING);
rover.send_pid_tuning(chan);
break;
default:
return GCS_MAVLINK::try_send_message(id);
}

1
APMrover2/GCS_Mavlink.h

@ -35,6 +35,7 @@ protected: @@ -35,6 +35,7 @@ protected:
uint64_t capabilities() const override;
void send_nav_controller_output() const override;
void send_pid_tuning() override;
private:

1
APMrover2/Rover.h

@ -436,7 +436,6 @@ private: @@ -436,7 +436,6 @@ private:
// GCS_Mavlink.cpp
void send_servo_out(mavlink_channel_t chan);
void send_pid_tuning(mavlink_channel_t chan);
void send_rpm(mavlink_channel_t chan);
void send_wheel_encoder_distance(mavlink_channel_t chan);

Loading…
Cancel
Save