mirror of https://github.com/ArduPilot/ardupilot
autotest: nicer display in FG quadcopter view
This commit is contained in:
parent
ae1ec05dd4
commit
9b654cd8fb
|
@ -71,20 +71,21 @@ Arducotper UAV Model
|
|||
</payload>
|
||||
<nasal>
|
||||
<arducopter>
|
||||
<file>Aircraft/arducopter/quad.nas</file>
|
||||
<script>
|
||||
setlistener("/sim/signals/fdm-initialized", func {
|
||||
var left = screen.display.new(20, 10);
|
||||
left.add("/engines/engine[0]/rpm");
|
||||
left.add("/engines/engine[1]/rpm");
|
||||
left.add("/engines/engine[2]/rpm");
|
||||
left.add("/engines/engine[3]/rpm");
|
||||
left.add("/apm/motor_right");
|
||||
left.add("/apm/motor_left");
|
||||
left.add("/apm/motor_front");
|
||||
left.add("/apm/motor_back");
|
||||
|
||||
var right = screen.display.new(-250, 20);
|
||||
right.add("/position/altitude-agl-ft");
|
||||
right.add("/orientation/roll-deg");
|
||||
right.add("/orientation/pitch-deg");
|
||||
right.add("/orientation/heading-deg");
|
||||
right.add("/instrumentation/gps/indicated-ground-speed-kt");
|
||||
right.add("/apm/altitude");
|
||||
right.add("/apm/roll");
|
||||
right.add("/apm/pitch");
|
||||
right.add("/apm/heading");
|
||||
right.add("/apm/airspeed");
|
||||
});
|
||||
</script>
|
||||
</arducopter>
|
||||
|
|
|
@ -0,0 +1,40 @@
|
|||
round10 = func(v) {
|
||||
if (v == nil) return 0;
|
||||
return 0.1*int(v*10);
|
||||
}
|
||||
|
||||
round100 = func(v) {
|
||||
if (v == nil) return 0;
|
||||
return 0.01*int(v*100);
|
||||
}
|
||||
|
||||
var update_quad = func( ) {
|
||||
asl_ft = getprop("/position/altitude-ft");
|
||||
ground = getprop("/position/ground-elev-ft");
|
||||
agl_m = (asl_ft - ground) * 0.3048;
|
||||
|
||||
setprop("/apm/altitude", round10(agl_m));
|
||||
|
||||
setprop("/apm/pitch", round10(getprop("/orientation/pitch-deg")));
|
||||
setprop("/apm/roll", round10(getprop("/orientation/roll-deg")));
|
||||
setprop("/apm/heading", round10(getprop("/orientation/heading-deg")));
|
||||
|
||||
# airspeed-kt is actually in feet per second (FDM NET bug)
|
||||
setprop("/apm/airspeed", round10(0.3048*getprop("/velocities/airspeed-kt")));
|
||||
|
||||
setprop("/apm/motor_left", round10(getprop("/engines/engine[0]/rpm")/10.0));
|
||||
setprop("/apm/motor_right", round10(getprop("/engines/engine[1]/rpm")/10.0));
|
||||
setprop("/apm/motor_front", round10(getprop("/engines/engine[2]/rpm")/10.0));
|
||||
setprop("/apm/motor_back", round10(getprop("/engines/engine[3]/rpm")/10.0));
|
||||
}
|
||||
|
||||
var main_loop = func {
|
||||
update_quad();
|
||||
settimer(main_loop, 0);
|
||||
}
|
||||
|
||||
|
||||
setlistener("/sim/signals/fdm-initialized",
|
||||
func {
|
||||
main_loop();
|
||||
});
|
Loading…
Reference in New Issue