From 09476bf9d3a9b60a0a4ddd7cd1f252bfa9dc4845 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 23 May 2015 08:57:00 +1000 Subject: [PATCH] Plane: added GCS_PID_MASK for realtime pid tuning --- ArduPlane/GCS_Mavlink.cpp | 47 +++++++++++++++++++++++++++++++++++++-- ArduPlane/Parameters.cpp | 7 ++++++ ArduPlane/Parameters.h | 2 ++ ArduPlane/Plane.h | 1 + 4 files changed, 55 insertions(+), 2 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 3a9020157b..ba2a6651a7 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -5,6 +5,8 @@ // default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control #define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS) +#define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN) + void Plane::send_heartbeat(mavlink_channel_t chan) { uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; @@ -90,7 +92,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan) void Plane::send_attitude(mavlink_channel_t chan) { - Vector3f omega = ahrs.get_gyro(); + const Vector3f &omega = ahrs.get_gyro(); mavlink_msg_attitude_send( chan, millis(), @@ -456,6 +458,40 @@ void Plane::send_wind(mavlink_channel_t chan) wind.z); } +/* + send PID tuning message + */ +void Plane::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 = rollController.get_pid_info(); + mavlink_msg_pid_tuning_send(chan, 1, + 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; + } + } + if (g.gcs_pid_mask & 2) { + const DataFlash_Class::PID_Info &pid_info = pitchController.get_pid_info(); + mavlink_msg_pid_tuning_send(chan, 2, + 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; + } + } +} + void Plane::send_rangefinder(mavlink_channel_t chan) { #if RANGEFINDER_ENABLED == ENABLED @@ -503,7 +539,6 @@ bool Plane::telemetry_delayed(mavlink_channel_t chan) // check if a message will fit in the payload space available #define CHECK_PAYLOAD_SIZE(id) if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_ ## id ## _LEN) return false - // try to send a message, return false if it won't fit in the serial tx buffer bool GCS_MAVLINK::try_send_message(enum ap_message id) { @@ -716,6 +751,11 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) case MSG_LIMITS_STATUS: // unused break; + + case MSG_PID_TUNING: + CHECK_PAYLOAD_SIZE(PID_TUNING); + plane.send_pid_tuning(chan); + break; } return true; } @@ -923,6 +963,9 @@ GCS_MAVLINK::data_stream_send(void) if (stream_trigger(STREAM_EXTRA1)) { send_message(MSG_ATTITUDE); send_message(MSG_SIMSTATE); + if (plane.control_mode != MANUAL) { + send_message(MSG_PID_TUNING); + } } if (plane.gcs_out_of_time) return; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 7fe42e8267..02310c040f 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -70,6 +70,13 @@ const AP_Param::Info Plane::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: KFF_RDDRMIX // @DisplayName: Rudder Mix // @Description: The amount of rudder mix to apply during aileron movement 0 = 0 %, 1 = 100% diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index ce20437e8d..08b017452f 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -138,6 +138,7 @@ public: k_param_glide_slope_threshold, k_param_rudder_only, k_param_gcs3, // 93 + k_param_gcs_pid_mask, // 100: Arming parameters k_param_arming = 100, @@ -482,6 +483,7 @@ public: #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 AP_Int8 override_channel; #endif + AP_Int8 gcs_pid_mask; // RC channels RC_Channel rc_1; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index bbddca18c9..77b4eb0d50 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -636,6 +636,7 @@ private: void send_simstate(mavlink_channel_t chan); void send_hwstatus(mavlink_channel_t chan); void send_wind(mavlink_channel_t chan); + void send_pid_tuning(mavlink_channel_t chan); void send_rangefinder(mavlink_channel_t chan); void send_current_waypoint(mavlink_channel_t chan); void send_statustext(mavlink_channel_t chan);