Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: added GCS_PID_MASK for mavlink rate tuning

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-23 10:04:54 +10:00
parent e79a21d8c2
commit 22524daf5f
3 changed files with 65 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
ArduCopter/GCS_Mavlink.pde
View File

@ -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;

2
ArduCopter/Parameters.h
View File

@ -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

7
ArduCopter/Parameters.pde
View File

@ -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.