c_meyer/pxt-ev3
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Drift-compensated angle in gyro (#931)

Browse Source 
* compute angle based on undrifted rate

* add is calibrating function

* fix integrator

* added example

* docs

* poll faster
This commit is contained in:
Peli de Halleux
2019-10-01 10:11:58 -07:00
committed by GitHub
parent 25452efc92
commit 374bbb8304
11 changed files with 192 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
sim/state/gyro.ts
View File

@ -8,7 +8,6 @@ namespace pxsim {
export class GyroSensorNode extends UartSensorNode {
id = NodeType.GyroSensor;
private angle: number = 0;
private rate: number = 0;
constructor(port: number) {
@ -19,14 +18,6 @@ namespace pxsim {
return DAL.DEVICE_TYPE_GYRO;
}
setAngle(angle: number) {
angle = angle | 0;
if (this.angle != angle) {
this.angle = angle;
this.setChangedState();
}
}
setRate(rate: number) {
rate = rate | 0;
if (this.rate != rate) {
@ -35,9 +26,12 @@ namespace pxsim {
}
}
getRate() {
return this.rate;
}
getValue() {
return this.mode == GyroSensorMode.Angle ? this.angle :
this.mode == GyroSensorMode.Rate ? this.rate : 0;
return this.getRate();
}
}
}

28
sim/visuals/controls/rotationSlider.ts
View File

@ -2,12 +2,11 @@
namespace pxsim.visuals {
const MAX_RATE = 40;
const MAX_ANGLE = 360;
export class RotationSliderControl extends ControlView<GyroSensorNode> {
private group: SVGGElement;
private slider: SVGGElement;
private text: SVGTextElement;
private rateText: SVGTextElement;
private static SLIDER_WIDTH = 70;
//private static SLIDER_HEIGHT = 78;
@ -26,8 +25,8 @@ namespace pxsim.visuals {
pxsim.svg.child(this.slider, "circle", { 'cx': 9, 'cy': 50, 'r': 13, 'style': 'fill: #f12a21' });
pxsim.svg.child(this.slider, "circle", { 'cx': 9, 'cy': 50, 'r': 12.5, 'style': 'fill: none;stroke: #b32e29' });
this.text = pxsim.svg.child(this.group, "text", {
'x': RotationSliderControl.SLIDER_WIDTH / 2,
this.rateText = pxsim.svg.child(this.group, "text", {
'x': this.getInnerWidth() / 2,
'y': RotationSliderControl.SLIDER_WIDTH * 1.2,
'text-anchor': 'middle', 'dominant-baseline': 'middle',
'style': 'font-size: 16px',
@ -72,17 +71,10 @@ namespace pxsim.visuals {
return;
}
const node = this.state;
let percentage = 50;
if (node.getMode() == GyroSensorMode.Rate) {
const rate = node.getValue();
this.text.textContent = `${rate}°/s`
// cap rate at 40deg/s
percentage = 50 + Math.sign(rate) * Math.min(MAX_RATE, Math.abs(rate)) / MAX_RATE * 50;
} else { //angle
const angle = node.getValue();
this.text.textContent = `${angle}°`
percentage = 50 + Math.sign(angle) * Math.min(MAX_ANGLE, Math.abs(angle)) / MAX_ANGLE * 50;
}
const rate = node.getRate();
this.rateText.textContent = `${rate}°/s`
// cap rate at 40deg/s
const percentage = 50 + Math.sign(rate) * Math.min(MAX_RATE, Math.abs(rate)) / MAX_RATE * 50;
const x = RotationSliderControl.SLIDER_WIDTH * percentage / 100;
const y = Math.abs((percentage - 50) / 50) * 10;
this.slider.setAttribute("transform", `translate(${x}, ${y})`);
@ -97,11 +89,7 @@ namespace pxsim.visuals {
t = -(t - 0.5) * 2; // [-1,1]
const state = this.state;
if (state.getMode() == GyroSensorMode.Rate) {
state.setRate(MAX_RATE * t);
} else {
state.setAngle(MAX_ANGLE * t)
}
state.setRate(MAX_RATE * t);
}
}