Skip to content

Commit

Permalink
remove gps shananigans, add githash in log when connecting
Browse files Browse the repository at this point in the history
  • Loading branch information
@tblaha
tblaha committed Sep 29, 2024
1 parent 38bf331 commit a3cd11c
Show file tree
Hide file tree
Showing 4 changed files with 81 additions and 221 deletions.
81 changes: 44 additions & 37 deletions src/js/msp/MSPHelper.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ import { showErrorDialog } from "../utils/showErrorDialog";
import GUI, { TABS } from "../gui";
import { OSD } from "../tabs/osd";
import { reinitializeConnection } from "../serial_backend";
import { gps, pos_set } from "../tabs/gps";
//import { gps, pos_set } from "../tabs/gps";

// Used for LED_STRIP
const ledDirectionLetters = ['n', 'e', 's', 'w', 'u', 'd']; // in LSB bit order
Expand Down Expand Up @@ -780,6 +780,13 @@ MspHelper.prototype.process_data = function(dataHandler) {
for (let i = 0; i < timeLength; i++) {
buff.push(data.readU8());
}
buff.push(32); // ascii space

const gitRevisionLength = 15;
for (let i = 0; i < gitRevisionLength; i++) {
buff.push(data.readU8());
}

FC.CONFIG.buildInfo = String.fromCharCode.apply(null, buff);
break;

Expand Down Expand Up @@ -2247,33 +2254,33 @@ MspHelper.prototype.crunch = function(code, modifierCode = undefined) {
buffer.push8(1);
break;

case MSPCodes.MSP2_SENSOR_GPS:
buffer.push8(0); // instance
buffer.push16(0); // gps week
buffer.push32(gps.time_ms); // ms_tow
buffer.push8(1); // fix_type
buffer.push8(20); // num sats
buffer.push16(1); // horizontal pos accuracy in cm
buffer.push16(1); // vertical pos accuracy in cm
buffer.push16(1); // horizontal vel accuracy in cm/s
buffer.push16(150); // dilusion of precision * 100
//buffer.push32(40_000_000); // longitude deg * 1e7
//buffer.push32(520_000_000); // latitude deg * 1e7
buffer.push32(gps.lon*1e7); // longitude deg * 1e7
buffer.push32(gps.lat*1e7); // latitude deg * 1e7
buffer.push32(gps.alt*100); // altitude in cm
buffer.push32(gps.vn*100); // ned vel north cm/s
buffer.push32(gps.ve*100); // ned vel east cm/s
buffer.push32(gps.vd*100); // ned vel down, unused
buffer.push16(0); // ground course, deg*10
buffer.push16(gps.yaw*10); // true yaw, deg*10, unused
buffer.push16(0); // year
buffer.push8(0); // month
buffer.push8(0); // day
buffer.push8(0); // hour
buffer.push8(0); // min
buffer.push8(0); // sec
break;
// case MSPCodes.MSP2_SENSOR_GPS:
// buffer.push8(0); // instance
// buffer.push16(0); // gps week
// buffer.push32(gps.time_ms); // ms_tow
// buffer.push8(1); // fix_type
// buffer.push8(20); // num sats
// buffer.push16(1); // horizontal pos accuracy in cm
// buffer.push16(1); // vertical pos accuracy in cm
// buffer.push16(1); // horizontal vel accuracy in cm/s
// buffer.push16(150); // dilusion of precision * 100
// //buffer.push32(40_000_000); // longitude deg * 1e7
// //buffer.push32(520_000_000); // latitude deg * 1e7
// buffer.push32(gps.lon*1e7); // longitude deg * 1e7
// buffer.push32(gps.lat*1e7); // latitude deg * 1e7
// buffer.push32(gps.alt*100); // altitude in cm
// buffer.push32(gps.vn*100); // ned vel north cm/s
// buffer.push32(gps.ve*100); // ned vel east cm/s
// buffer.push32(gps.vd*100); // ned vel down, unused
// buffer.push16(0); // ground course, deg*10
// buffer.push16(gps.yaw*10); // true yaw, deg*10, unused
// buffer.push16(0); // year
// buffer.push8(0); // month
// buffer.push8(0); // day
// buffer.push8(0); // hour
// buffer.push8(0); // min
// buffer.push8(0); // sec
// break;

case MSPCodes.MSP2_SET_INDI_CONFIG:
break;
Expand All @@ -2285,15 +2292,15 @@ MspHelper.prototype.crunch = function(code, modifierCode = undefined) {
case MSPCodes.MSP2_SET_LEARNING_CONFIG:
break;

case MSPCodes.MSP2_SET_POSITION_SETPOINT:
buffer.push32(pos_set.time_ms);
buffer.push8(pos_set.mode);
buffer.push32(1000*pos_set.x);
buffer.push32(1000*pos_set.y);
buffer.push32(1000*pos_set.z);
buffer.push16(10*pos_set.yaw);
buffer.push32(100*pos_set.vtraj);
break;
// case MSPCodes.MSP2_SET_POSITION_SETPOINT:
// buffer.push32(pos_set.time_ms);
// buffer.push8(pos_set.mode);
// buffer.push32(1000*pos_set.x);
// buffer.push32(1000*pos_set.y);
// buffer.push32(1000*pos_set.z);
// buffer.push16(10*pos_set.yaw);
// buffer.push32(100*pos_set.vtraj);
// break;

case MSPCodes.MSP_SET_SIMPLIFIED_TUNING:
MspHelper.writePidSliderSettings(buffer);
Expand Down
2 changes: 1 addition & 1 deletion src/js/serial_backend.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -489,7 +489,7 @@ async function processBuildConfiguration() {
await MSP.promise(MSPCodes.MSP2_GET_TEXT, mspHelper.crunch(MSPCodes.MSP2_GET_TEXT, MSPCodes.BUILD_KEY));

if (FC.CONFIG.buildKey.length === 32 && navigator.onLine) {
gui_log(i18n.getMessage('buildKey', FC.CONFIG.buildKey));
//gui_log(i18n.getMessage('buildKey', FC.CONFIG.buildKey));
buildApi.requestBuildOptions(FC.CONFIG.buildKey, onLoadCloudBuild, processCraftName);
} else {
processCraftName();
Expand Down
178 changes: 36 additions & 142 deletions src/js/tabs/gps.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,34 +8,8 @@ import MSPCodes from "../msp/MSPCodes";
import { have_sensor } from "../sensor_helpers";
import { mspHelper } from '../msp/MSPHelper';
import { updateTabList } from '../utils/updateTabList';
//import { checkChromeRuntimeError } from "./utils/common";

const gps = {
time_ms: 0,
lat: 0,
lon: 0,
alt: 0,
vn: 0,
ve: 0,
vd: 0,
yaw: 0,
groundCourse: 0,
MSPrelay: false,
};

const pos_set = {
time_ms: 0,
mode: 0,
x: 0,
y: 0,
z: -1.5,
lat: 0,
lon: 0,
alt: 0,
yaw: 0,
vtraj: 0,
};

const gps = {};
gps.initialize = async function (callback) {

GUI.active_tab = 'gps';
Expand All @@ -45,86 +19,6 @@ gps.initialize = async function (callback) {

const hasMag = have_sensor(FC.CONFIG.activeSensors, 'mag');

const HOME_LAT = 51.99071002805145;
const HOME_LON = 4.376727452462819;
const RE = 6378137.;

pos_set.time_ms = 0;
pos_set.mode = 0; // 0: pos control, 1: trajectory start point, 2: trajectory run
pos_set.lat = HOME_LAT + 180. / 3.1415 * (pos_set.x / RE);
pos_set.lon = HOME_LON + 180. / 3.1415 * (pos_set.y / RE) / Math.cos(HOME_LON * 3.1415/180.);
//pos_set.alt = pos_set.alt;
pos_set.yaw = 0;
pos_set.vtraj = 0; // if mode == 2: speed of the trajectory

if (gps.MSPrelay == false) {
chrome.sockets.udp.create({}, function(socketInfo) {
let socketId = socketInfo.socketId;

// Bind the socket to a local address and port
chrome.sockets.udp.bind(socketId, "127.0.0.1", 5769, function(result) {
if (result < 0) {
console.log("Error binding socket: ");
gps.MSPrelay = false;
return;
}
gps.MSPrelay = true;
console.log("Socket bound");
});

// Listen for incoming packets
chrome.sockets.udp.onReceive.addListener(function(info) {
let data = new Uint8Array(info.data);
//var message = String.fromCharCode.apply(null, data);
//console.log("Received message: " + message);

// Assuming each data type has fixed byte sizes
let id = new Uint32Array(data.buffer, 0, 1)[0];
let arr = new Uint32Array(data.buffer, 4, 2);
/* global BigInt */
let timeUs1 = ((BigInt)(arr[1]) << 32n) + (BigInt)(arr[0]);
let timeMs1 = (Number)(timeUs1 / 1000n);

let x = new Float32Array(data.buffer, 12, 1)[0];
let y = new Float32Array(data.buffer, 16, 1)[0];
let z = new Float32Array(data.buffer, 20, 1)[0];
let qx = new Float32Array(data.buffer, 24, 1)[0];
let qy = new Float32Array(data.buffer, 28, 1)[0];
let qz = new Float32Array(data.buffer, 32, 1)[0];
let qw = new Float32Array(data.buffer, 36, 1)[0];
//var arr = new Uint32Array(data.buffer, 40, 2);
//var timeUs2 = new BigUint64Array(arr.buffer, 0, 1)[0];
let vx = new Float32Array(data.buffer, 48, 1)[0];
let vy = new Float32Array(data.buffer, 52, 1)[0];
let vz = new Float32Array(data.buffer, 56, 1)[0];
//var wx = new Float32Array(data.buffer, 60, 1)[0];
//var wy = new Float32Array(data.buffer, 64, 1)[0];
//var wz = new Float32Array(data.buffer, 68, 1)[0];

gps.time_ms = timeMs1;
gps.lat = HOME_LAT + 180. / 3.1415 * (x / RE);
gps.lon = HOME_LON + 180. / 3.1415 * (y / RE) / Math.cos(HOME_LON * 3.1415/180.);
gps.alt = -z;
gps.vn = vx;
gps.ve = vy;
gps.vd = vz;
gps.yaw = 180. / 3.1415 * Math.atan2( 2. * (qw*qz + qx*qy), 1. - 2. * (qy*qy+qz*qz) );
gps.groundCourse = 180. / 3.1415 * Math.atan2(vx, vy);

MSP.send_message(MSPCodes.MSP2_SENSOR_GPS,
mspHelper.crunch(MSPCodes.MSP2_SENSOR_GPS), false, false, false);
//MSP.send_message(MSPCodes.MSP2_SET_POSITION_SETPOINT,
// mspHelper.crunch(MSPCodes.MSP2_SET_POSITION_SETPOINT), false, false, false);
});

// Error listener
chrome.sockets.udp.onReceiveError.addListener(function(error) {
console.log("Error receiving data: ");
gps.MSPrelay = false;
});
});
}

load_html();

function load_html() {
Expand Down Expand Up @@ -282,40 +176,40 @@ gps.initialize = async function (callback) {

// End GPS Configuration

// Setpoint configuration
const setpointModes = [
"position",
"trajectory",
];

const setpointModeElement = $('select.pos_set_mode');
for (const mode of setpointModes) {
setpointModeElement.append(`<option value="${mode}">${mode}</option>`);
}
console.log("setup done");

function sendSetpoint(mode) {
pos_set.time_ms = gps.time_ms;
pos_set.x = $('input[name="pos_set_x"]').val();
pos_set.y = $('input[name="pos_set_y"]').val();
pos_set.lat = HOME_LAT + 180. / 3.1415 * (pos_set.x / RE);
pos_set.lon = HOME_LON + 180. / 3.1415 * (pos_set.y / RE) / Math.cos(HOME_LON * 3.1415/180.);
pos_set.z = $('input[name="pos_set_z"]').val();
pos_set.mode = mode;
pos_set.vtraj = $('input[name="pos_set_vtraj"]').val();
pos_set.yaw = $('input[name="pos_set_yaw"]').val();
MSP.send_message(MSPCodes.MSP2_SET_POSITION_SETPOINT,
mspHelper.crunch(MSPCodes.MSP2_SET_POSITION_SETPOINT), false, false, false);
}

$('input[name="pos_set_vtraj"]').on('change', function() { sendSetpoint(2); });
$('a.sendSetpoint').on('click', function() { sendSetpoint(0); });
$('a.traj_start').on('click', function() { sendSetpoint(1); });
$('a.traj_pause').on('click', function() {
$('input[name="pos_set_vtraj"]').val(0);
sendSetpoint(2);
});

// // Setpoint configuration
// const setpointModes = [
// "position",
// "trajectory",
// ];
//
// const setpointModeElement = $('select.pos_set_mode');
// for (const mode of setpointModes) {
// setpointModeElement.append(`<option value="${mode}">${mode}</option>`);
// }
// console.log("setup done");
//
// function sendSetpoint(mode) {
// pos_set.time_ms = gps.time_ms;
// pos_set.x = $('input[name="pos_set_x"]').val();
// pos_set.y = $('input[name="pos_set_y"]').val();
// pos_set.lat = HOME_LAT + 180. / 3.1415 * (pos_set.x / RE);
// pos_set.lon = HOME_LON + 180. / 3.1415 * (pos_set.y / RE) / Math.cos(HOME_LON * 3.1415/180.);
// pos_set.z = $('input[name="pos_set_z"]').val();
// pos_set.mode = mode;
// pos_set.vtraj = $('input[name="pos_set_vtraj"]').val();
// pos_set.yaw = $('input[name="pos_set_yaw"]').val();
// MSP.send_message(MSPCodes.MSP2_SET_POSITION_SETPOINT,
// mspHelper.crunch(MSPCodes.MSP2_SET_POSITION_SETPOINT), false, false, false);
// }
//
// $('input[name="pos_set_vtraj"]').on('change', function() { sendSetpoint(2); });
// $('a.sendSetpoint').on('click', function() { sendSetpoint(0); });
// $('a.traj_start').on('click', function() { sendSetpoint(1); });
// $('a.traj_pause').on('click', function() {
// $('input[name="pos_set_vtraj"]').val(0);
// sendSetpoint(2);
// });
//
function update_ui() {
const lat = FC.GPS_DATA.lat / 10000000;
const lon = FC.GPS_DATA.lon / 10000000;
Expand Down Expand Up @@ -524,5 +418,5 @@ gps.cleanup = function (callback) {

TABS.gps = gps;
export {
gps, pos_set,
gps,
};
41 changes: 0 additions & 41 deletions src/tabs/gps.html
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,47 +120,6 @@
</table>
</div>
</div>
<div class="gui_box grey">
<div class="gui_box_titlebar">
<div class="spacer_box_title">Setpoints</div>
<div class="helpicon cf_tip"></div>
</div>
<div class="spacer_box">
<div class="pos_set">
<div style="float:left; width:100%;">
<div class="select line">
<select class="pos_set_mode">
<!-- list generated here -->
</select>
<span>Mode</span>
</div>
<div class="pos_set_test">
<input type="number" name="pos_set_x" step="0.01" min="-50" max="50">
<input type="number" name="pos_set_y" step="0.01" min="-50" max="50">
<input type="number" name="pos_set_z" step="0.01" min="-50" max="50">
<label for="pos_set_z">
<span>Position NED</span>
</label>
<input type="number" name="pos_set_yaw" step="1" min="-180" max="180">
<label for="pos_set_yaw">
<span>Yaw</span>
</label>
<a class="sendSetpoint" href="#">Send</a>
<div class="helpicon cf_tip i18n_title-replaced"></div>
</div>
<div class="pos_set_test2">
<input type="number" name="pos_set_vtraj" step="0.5" min="-50" max="50">
<label for="pos_set_vtraj">
<span>Trajectory Speed</span>
</label>
<a class="traj_start" href="#">To start point</a>
<a class="traj_pause" href="#">Pause</a>
<div class="helpicon cf_tip i18n_title-replaced"></div>
</div>
</div>
</div>
</div>
</div>
<div class="gui_box grey gps_map">
<div class="gui_box_titlebar" style="margin-bottom: 0px;">
<div class="spacer_box_title" i18n="gpsMapHead"></div>
Expand Down

0 comments on commit a3cd11c

Please sign in to comment.