Skip to content

PIDReview: add support for angle gains #333

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
IamPete1 wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

PIDReview: add support for angle gains #333

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
103 changes: 70 additions & 33 deletions PIDReview/PIDReview.js
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,8 @@ function reset() {
const types = ["PIDP", "PIDR", "PIDY",
"PIQP", "PIQR", "PIQY",
"RATE_R", "RATE_P", "RATE_Y",
"PIDS", "PIDA"]
"PIDS", "PIDA",
"ANG_Roll", "ANG_Pitch", "ANG_Yaw"]
for (const type of types) {
let ele = document.getElementById("type_" + type)
ele.disabled = true
Expand Down Expand Up @@ -618,7 +619,7 @@ function add_param_sets() {
item.appendChild(document.createTextNode("Show"))
set_cell_style(item)

const names = get_PID_param_names(PID.params.prefix)
const names = PID.param_getter(PID.params.prefix)
for (const [name, param] of Object.entries(names)) {
let item = document.createElement("th")
header.appendChild(item)
Expand Down Expand Up @@ -711,15 +712,18 @@ function add_param_sets() {
document.getElementById("Spec_Act").disabled = false
document.getElementById("Spec_Out").disabled = false

// Only have others from a full PID log
const have_all = PID.id[0] !== "RATE"
document.getElementById("PIDX_Err").disabled = !have_all
// Full PID logs have Err, P, I, D, FF; ANG has only Err; RATE has none
const is_ANG = PID.id[0] === "ANG"
const have_all = PID.id[0] !== "RATE" && !is_ANG
const have_err = have_all || is_ANG

document.getElementById("PIDX_Err").disabled = !have_err
document.getElementById("PIDX_P").disabled = !have_all
document.getElementById("PIDX_I").disabled = !have_all
document.getElementById("PIDX_D").disabled = !have_all
document.getElementById("PIDX_FF").disabled = !have_all

document.getElementById("Spec_Err").disabled = !have_all
document.getElementById("Spec_Err").disabled = !have_err
document.getElementById("Spec_P").disabled = !have_all
document.getElementById("Spec_I").disabled = !have_all
document.getElementById("Spec_D").disabled = !have_all
Expand All @@ -731,8 +735,10 @@ function add_param_sets() {
document.getElementById("Spec_DFF").disabled = !have_DFF

// Uncheck any that are disabled
if (!have_all) {
if (!have_err) {
document.getElementById("PIDX_Err").checked = false
}
if (!have_all) {
document.getElementById("PIDX_P").checked = false
document.getElementById("PIDX_I").checked = false
document.getElementById("PIDX_D").checked = false
Expand All @@ -742,14 +748,14 @@ function add_param_sets() {
document.getElementById("PIDX_DFF").checked = false
}

// Change to Out on spectrogram if disabled option is set
const disabled_checked = document.getElementById("Spec_Err").checked ||
document.getElementById("Spec_P").checked ||
document.getElementById("Spec_I").checked ||
document.getElementById("Spec_D").checked ||
document.getElementById("Spec_FF").checked ||
document.getElementById("Spec_DFF").checked
if ((!have_all || !have_DFF) && disabled_checked) {
// Change to Out on spectrogram if the selected option is now disabled
const disabled_checked = (!have_err && document.getElementById("Spec_Err").checked) ||
(!have_all && (document.getElementById("Spec_P").checked ||
document.getElementById("Spec_I").checked ||
document.getElementById("Spec_D").checked ||
document.getElementById("Spec_FF").checked)) ||
(!have_DFF && document.getElementById("Spec_DFF").checked)
if (disabled_checked) {
document.getElementById("Spec_Out").checked = true
}

Expand Down Expand Up @@ -1308,7 +1314,7 @@ function time_range_changed() {
}

function get_PID_param_names(prefix) {
return {
return {
KP: {
title: "KP",
name: prefix + "P",
Expand Down Expand Up @@ -1372,6 +1378,16 @@ function get_PID_param_names(prefix) {
}
}

function get_ANG_param_names(prefix) {
return {
KP: {
title: "KP",
name: prefix + "P",
decimalPlaces: 4,
}
}
}

// Split use the given time array to return split points in log data
// Split at any change in parameters
// Split at any dropped data
Expand Down Expand Up @@ -1466,26 +1482,32 @@ async function load(log_file) {

case 2: // Copter
PID_log_messages = [
{id: ["PIDR"], prefixes: ["ATC_RAT_RLL_"], unitScale: rad2deg, units: degUnits },
{id: ["PIDP"], prefixes: ["ATC_RAT_PIT_"], unitScale: rad2deg, units: degUnits },
{id: ["PIDY"], prefixes: ["ATC_RAT_YAW_"], unitScale: rad2deg, units: degUnits },
{id: ["RATE", "R"], prefixes: ["ATC_RAT_RLL_"], unitScale: rad2deg, units: degUnits },
{id: ["RATE", "P"], prefixes: ["ATC_RAT_PIT_"], unitScale: rad2deg, units: degUnits },
{id: ["RATE", "Y"], prefixes: ["ATC_RAT_YAW_"], unitScale: rad2deg, units: degUnits }
{id: ["PIDR"], prefixes: ["ATC_RAT_RLL_"], unitScale: rad2deg, units: degUnits, param_getter: get_PID_param_names },
{id: ["PIDP"], prefixes: ["ATC_RAT_PIT_"], unitScale: rad2deg, units: degUnits, param_getter: get_PID_param_names },
{id: ["PIDY"], prefixes: ["ATC_RAT_YAW_"], unitScale: rad2deg, units: degUnits, param_getter: get_PID_param_names },
{id: ["RATE", "R"], prefixes: ["ATC_RAT_RLL_"], unitScale: rad2deg, units: degUnits, param_getter: get_PID_param_names },
{id: ["RATE", "P"], prefixes: ["ATC_RAT_PIT_"], unitScale: rad2deg, units: degUnits, param_getter: get_PID_param_names },
{id: ["RATE", "Y"], prefixes: ["ATC_RAT_YAW_"], unitScale: rad2deg, units: degUnits, param_getter: get_PID_param_names },
{id: ["ANG", "Roll"], prefixes: ["ATC_ANG_RLL_"], unitScale: 1.0, units: "deg", param_getter: get_ANG_param_names },
{id: ["ANG", "Pitch"], prefixes: ["ATC_ANG_PIT_"], unitScale: 1.0, units: "deg", param_getter: get_ANG_param_names },
{id: ["ANG", "Yaw"], prefixes: ["ATC_ANG_YAW_"], unitScale: 1.0, units: "deg", param_getter: get_ANG_param_names },
]
break

case 3: // Plane
PID_log_messages = [
{id: ["PIDR"], prefixes: ["RLL_RATE_"], unitScale: 1.0, units: degUnits },
{id: ["PIDP"], prefixes: ["PTCH_RATE_"], unitScale: 1.0, units: degUnits },
{id: ["PIDY"], prefixes: ["YAW_RATE_"], unitScale: 1.0, units: degUnits },
{id: ["PIQR"], prefixes: ["Q_A_RAT_RLL_"], unitScale: rad2deg, units: degUnits },
{id: ["PIQP"], prefixes: ["Q_A_RAT_PIT_"], unitScale: rad2deg, units: degUnits },
{id: ["PIQY"], prefixes: ["Q_A_RAT_YAW_"], unitScale: rad2deg, units: degUnits },
{id: ["RATE", "R"], prefixes: ["Q_A_RAT_RLL_"], unitScale: rad2deg, units: degUnits },
{id: ["RATE", "P"], prefixes: ["Q_A_RAT_PIT_"], unitScale: rad2deg, units: degUnits },
{id: ["RATE", "Y"], prefixes: ["Q_A_RAT_YAW_"], unitScale: rad2deg, units: degUnits },
{id: ["PIDR"], prefixes: ["RLL_RATE_"], unitScale: 1.0, units: degUnits, param_getter: get_PID_param_names },
{id: ["PIDP"], prefixes: ["PTCH_RATE_"], unitScale: 1.0, units: degUnits, param_getter: get_PID_param_names },
{id: ["PIDY"], prefixes: ["YAW_RATE_"], unitScale: 1.0, units: degUnits, param_getter: get_PID_param_names },
{id: ["PIQR"], prefixes: ["Q_A_RAT_RLL_"], unitScale: rad2deg, units: degUnits, param_getter: get_PID_param_names },
{id: ["PIQP"], prefixes: ["Q_A_RAT_PIT_"], unitScale: rad2deg, units: degUnits, param_getter: get_PID_param_names },
{id: ["PIQY"], prefixes: ["Q_A_RAT_YAW_"], unitScale: rad2deg, units: degUnits, param_getter: get_PID_param_names },
{id: ["RATE", "R"], prefixes: ["Q_A_RAT_RLL_"], unitScale: rad2deg, units: degUnits, param_getter: get_PID_param_names },
{id: ["RATE", "P"], prefixes: ["Q_A_RAT_PIT_"], unitScale: rad2deg, units: degUnits, param_getter: get_PID_param_names },
{id: ["RATE", "Y"], prefixes: ["Q_A_RAT_YAW_"], unitScale: rad2deg, units: degUnits, param_getter: get_PID_param_names },
{id: ["ANG", "Roll"], prefixes: ["Q_A_ANG_RLL_"], unitScale: 1.0, units: "deg", param_getter: get_ANG_param_names },
{id: ["ANG", "Pitch"], prefixes: ["Q_A_ANG_PIT_"], unitScale: 1.0, units: "deg", param_getter: get_ANG_param_names },
{id: ["ANG", "Yaw"], prefixes: ["Q_A_ANG_YAW_"], unitScale: 1.0, units: "deg", param_getter: get_ANG_param_names },
]
break

Expand Down Expand Up @@ -1513,7 +1535,7 @@ async function load(log_file) {
PID_log_messages[i].params = { prefix: null, sets: [] }
for (const prefix of PID_log_messages[i].prefixes) {

const names = get_PID_param_names(prefix)
const names = PID_log_messages[i].param_getter(prefix)

let param_values = { start_time: 0 }
for (const name in names) {
Expand Down Expand Up @@ -1579,6 +1601,7 @@ async function load(log_file) {
const log_msg = log.get(id)

const is_RATE_msg = id === "RATE"
const is_ANG_msg = id === "ANG"

const time = TimeUS_to_seconds(log_msg.TimeUS)

Expand All @@ -1591,7 +1614,21 @@ async function load(log_file) {
if (PID_log_messages[i].sets[batch.param_set] == null) {
PID_log_messages[i].sets[batch.param_set] = []
}
if (is_RATE_msg) {
if (is_ANG_msg) {
const axis_name = PID_log_messages[i].id[1] // "Roll", "Pitch", or "Yaw"
const tar_data = Array.from(log_msg["Des" + axis_name].slice(batch.batch_start, batch.batch_end))
const act_data = Array.from(log_msg[axis_name].slice(batch.batch_start, batch.batch_end))
const err_data = array_sub(tar_data, act_data)
// Output is angle P gain * error, giving the commanded rate target (deg/s)
const kp = PID_log_messages[i].params.sets[batch.param_set].KP ?? 0
PID_log_messages[i].sets[batch.param_set].push({ time: time.slice(batch.batch_start, batch.batch_end),
sample_rate: batch.sample_rate,
Tar: tar_data,
Act: act_data,
Err: err_data,
Out: array_scale(err_data, kp) })

} else if (is_RATE_msg) {
const axis_prefix = PID_log_messages[i].id[1]
// Note that is not quite the same, PID logs report the filtered target value where as RATE gets the raw
PID_log_messages[i].sets[batch.param_set].push({ time: time.slice(batch.batch_start, batch.batch_end),
Expand Down
12 changes: 10 additions & 2 deletions PIDReview/index.html
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ <h1 style="text-align:center"><a href="" style="color: #000000; text-decoration:
</fieldset>
</td>
<td>
<fieldset style="width:200px;height:80px">
<fieldset style="width:150px;height:80px">
<legend>Analysis time</legend>
<label for="TimeStart">Start (s)</label>
<input id="TimeStart" name="TimeStart" type="number" min="0" step="1" value="0" onchange="time_range_changed()" style="width:50px"/><br><br>
Expand All @@ -73,7 +73,7 @@ <h1 style="text-align:center"><a href="" style="color: #000000; text-decoration:
</fieldset>
</td>
<td>
<fieldset style="width:310px;height:80px">
<fieldset style="width:410px;height:80px">
<legend>Axis</legend>
<table>
<td>
Expand Down Expand Up @@ -106,6 +106,14 @@ <h1 style="text-align:center"><a href="" style="color: #000000; text-decoration:
<input type="radio" id="type_PIDA" name="Axis" onchange="loading_call(setup_axis)">
<label for="type_PIDA">PIDA</label><br>
</td>
<td>
<input type="radio" id="type_ANG_Roll" name="Axis" onchange="loading_call(setup_axis)">
<label for="type_ANG_Roll">ANG Roll</label><br>
<input type="radio" id="type_ANG_Pitch" name="Axis" onchange="loading_call(setup_axis)">
<label for="type_ANG_Pitch">ANG Pitch</label><br>
<input type="radio" id="type_ANG_Yaw" name="Axis" onchange="loading_call(setup_axis)">
<label for="type_ANG_Yaw">ANG Yaw</label><br>
</td>
</table>
</fieldset>
</td>
Expand Down