diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 0a06e67771..9ceb807460 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -14,6 +14,7 @@ static bool gcs_out_of_time; // check if a message will fit in the payload space available +#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN) #define CHECK_PAYLOAD_SIZE(id) if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN) return false static void gcs_send_heartbeat(void) @@ -421,6 +422,55 @@ static void NOINLINE send_rangefinder(mavlink_channel_t chan) } #endif + +/* + send PID tuning message + */ +static void send_pid_tuning(mavlink_channel_t chan) +{ + const Vector3f &gyro = ahrs.get_gyro(); + if (g.gcs_pid_mask & 1) { + const DataFlash_Class::PID_Info &pid_info = g.pid_rate_roll.get_pid_info(); + mavlink_msg_pid_tuning_send(chan, 1, + pid_info.desired*0.01f, + degrees(gyro.x), + pid_info.FF*0.01, + pid_info.P*0.01, + pid_info.I*0.01, + pid_info.D*0.01); + if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) { + return; + } + } + if (g.gcs_pid_mask & 2) { + const DataFlash_Class::PID_Info &pid_info = g.pid_rate_pitch.get_pid_info(); + mavlink_msg_pid_tuning_send(chan, 2, + pid_info.desired*0.01f, + degrees(gyro.y), + pid_info.FF*0.01f, + pid_info.P*0.01f, + pid_info.I*0.01f, + pid_info.D*0.01f); + if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) { + return; + } + } + if (g.gcs_pid_mask & 4) { + const DataFlash_Class::PID_Info &pid_info = g.pid_rate_yaw.get_pid_info(); + mavlink_msg_pid_tuning_send(chan, 3, + pid_info.desired*0.01f, + degrees(gyro.z), + pid_info.FF*0.01f, + pid_info.P*0.01f, + pid_info.I*0.01f, + pid_info.D*0.01f); + if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) { + return; + } + } +} + + static void NOINLINE send_statustext(mavlink_channel_t chan) { mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status; @@ -659,6 +709,11 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) // unused break; + case MSG_PID_TUNING: + CHECK_PAYLOAD_SIZE(PID_TUNING); + send_pid_tuning(chan); + break; + case MSG_RETRY_DEFERRED: break; // just here to prevent a warning } @@ -859,6 +914,7 @@ GCS_MAVLINK::data_stream_send(void) if (stream_trigger(STREAM_EXTRA1)) { send_message(MSG_ATTITUDE); send_message(MSG_SIMSTATE); + send_message(MSG_PID_TUNING); } if (gcs_out_of_time) return; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index d67fb13d9e..62872fc455 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -194,6 +194,7 @@ public: k_param_ch12_option, // 123 k_param_takeoff_trigger_dz, k_param_gcs3, // 125 + k_param_gcs_pid_mask, // // 140: Sensor parameters @@ -414,6 +415,7 @@ public: AP_Int8 land_repositioning; AP_Float ekfcheck_thresh; + AP_Int8 gcs_pid_mask; #if FRAME_CONFIG == HELI_FRAME // Heli diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 5250b29e66..36ec914526 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -108,6 +108,13 @@ const AP_Param::Info var_info[] PROGMEM = { // @Increment: 1 GSCALAR(telem_delay, "TELEM_DELAY", 0), + // @Param: GCS_PID_MASK + // @DisplayName: GCS PID tuning mask + // @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for + // @User: Advanced + // @Values: 0:None,1:Roll,2:Pitch,4:Yaw + GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0), + // @Param: RTL_ALT // @DisplayName: RTL Altitude // @Description: The minimum altitude the model will move to before Returning to Launch. Set to zero to return at current altitude.