|
|
|
|
@ -0,0 +1,683 @@
|
|
|
|
|
/**
|
|
|
|
|
* MAVROS 通信模块 - 基于 roslibjs
|
|
|
|
|
* 直接通过 MAVROS 话题和服务控制无人机
|
|
|
|
|
*
|
|
|
|
|
* 架构: 浏览器 ──roslibjs──► rosbridge_server ──ROS话题──► MAVROS ──MAVLink──► PX4
|
|
|
|
|
*
|
|
|
|
|
* 使用的话题:
|
|
|
|
|
* 订阅: /uav1/mavros/state (mavros_msgs/State)
|
|
|
|
|
* 订阅: /uav1/mavros/local_position/pose (geometry_msgs/PoseStamped)
|
|
|
|
|
* 订阅: /uav1/mavros/global_position/raw/fix (sensor_msgs/NavSatFix)
|
|
|
|
|
* 订阅: /uav1/mavros/battery (sensor_msgs/BatteryState)
|
|
|
|
|
* 发布: /uav1/mavros/setpoint_position/local (geometry_msgs/PoseStamped)
|
|
|
|
|
* 服务: /uav1/mavros/cmd/arming (mavros_msgs/CommandBool)
|
|
|
|
|
* 服务: /uav1/mavros/set_mode (mavros_msgs/SetMode)
|
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
const WsModule = (() => {
|
|
|
|
|
let ros = null;
|
|
|
|
|
let rosConnected = false;
|
|
|
|
|
let droneConnected = false;
|
|
|
|
|
let fcuReady = false; // true when MAVROS reports valid mode (FCU connection established)
|
|
|
|
|
|
|
|
|
|
// ========== 配置: UAV 命名空间 ==========
|
|
|
|
|
// 仿真环境默认: '/uav1' 实机可能: '' 或 '/uav1'
|
|
|
|
|
// 如果话题不对，改这里
|
|
|
|
|
let uavNS = '/uav1';
|
|
|
|
|
|
|
|
|
|
function topicName(name) { return uavNS + name; }
|
|
|
|
|
function serviceName(name) { return uavNS + name; }
|
|
|
|
|
|
|
|
|
|
// ROS handles
|
|
|
|
|
let stateSub = null;
|
|
|
|
|
let localPosSub = null;
|
|
|
|
|
let gpsSub = null;
|
|
|
|
|
let batterySub = null;
|
|
|
|
|
let setpointPub = null;
|
|
|
|
|
|
|
|
|
|
// Telemetry state
|
|
|
|
|
let telemetry = {
|
|
|
|
|
lat: 0, lng: 0, alt: 0,
|
|
|
|
|
speed: 0, battery: 100, heading: 0,
|
|
|
|
|
flightMode: 'UNKNOWN', gpsStatus: 'No Fix',
|
|
|
|
|
armed: false,
|
|
|
|
|
localPos: { x: 0, y: 0, z: 0 } // ENU: x=East, y=North, z=Up
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
// Origin for lat/lng <-> ENU conversion
|
|
|
|
|
let origin = { lat: null, lng: null };
|
|
|
|
|
let originSet = false;
|
|
|
|
|
let originFromGps = false; // true if origin was set from real GPS
|
|
|
|
|
|
|
|
|
|
// Navigation state
|
|
|
|
|
let rawWaypoints = []; // Original lat/lng waypoints
|
|
|
|
|
let navWaypoints = []; // Converted ENU waypoints
|
|
|
|
|
let currentWPIndex = 0;
|
|
|
|
|
let navActive = false;
|
|
|
|
|
let navPaused = false;
|
|
|
|
|
let publishTimer = null;
|
|
|
|
|
let wpCheckTimer = null;
|
|
|
|
|
let telemetryTimer = null; // Throttled UI update timer
|
|
|
|
|
|
|
|
|
|
const WP_REACH_DIST = 2.0; // meters
|
|
|
|
|
const PUBLISH_INTERVAL = 100; // ms (10 Hz, OFFBOARD needs >= 2Hz)
|
|
|
|
|
|
|
|
|
|
// ---- Coordinate conversion ----
|
|
|
|
|
|
|
|
|
|
function latLngToENU(lat, lng, alt) {
|
|
|
|
|
if (!originSet) return { x: 0, y: 0, z: alt };
|
|
|
|
|
const latRad = origin.lat * Math.PI / 180;
|
|
|
|
|
const x = (lng - origin.lng) * Math.cos(latRad) * 111319.488; // East
|
|
|
|
|
const y = (lat - origin.lat) * 111319.488; // North
|
|
|
|
|
return { x, y, z: alt }; // Up
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function enuToLatLng(x, y) {
|
|
|
|
|
if (!originSet) return { lat: 0, lng: 0 };
|
|
|
|
|
const latRad = origin.lat * Math.PI / 180;
|
|
|
|
|
const lat = origin.lat + y / 111319.488;
|
|
|
|
|
const lng = origin.lng + x / (111319.488 * Math.cos(latRad));
|
|
|
|
|
return { lat, lng };
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Quaternion to yaw (degrees, ENU: 0=East, CCW+)
|
|
|
|
|
function quatToYawDeg(qx, qy, qz, qw) {
|
|
|
|
|
const siny = 2 * (qw * qz + qx * qy);
|
|
|
|
|
const cosy = 1 - 2 * (qy * qy + qz * qz);
|
|
|
|
|
let yaw = Math.atan2(siny, cosy) * 180 / Math.PI;
|
|
|
|
|
// Convert to compass heading: 0=North, CW+
|
|
|
|
|
return (90 - yaw + 360) % 360;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// ---- Init & Connection ----
|
|
|
|
|
|
|
|
|
|
function init() {}
|
|
|
|
|
|
|
|
|
|
function isConnected() { return rosConnected; }
|
|
|
|
|
|
|
|
|
|
function connectDrone(url) {
|
|
|
|
|
try {
|
|
|
|
|
if (typeof ROSLIB === 'undefined') {
|
|
|
|
|
UIModule.onStatus({ type: 'error', message: 'roslibjs 库未加载' });
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (ros) {
|
|
|
|
|
try { ros.close(); } catch (e) { /* ignore */ }
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
UIModule.onStatus({ type: 'info', message: `正在连接 rosbridge: ${url} ...` });
|
|
|
|
|
|
|
|
|
|
ros = new ROSLIB.Ros({ url: url });
|
|
|
|
|
|
|
|
|
|
ros.on('connection', () => {
|
|
|
|
|
rosConnected = true;
|
|
|
|
|
UIModule.onStatus({ type: 'info', message: 'rosbridge 已连接，正在检测 MAVROS 话题...' });
|
|
|
|
|
setupSubscriptions();
|
|
|
|
|
});
|
|
|
|
|
|
|
|
|
|
ros.on('error', (error) => {
|
|
|
|
|
UIModule.onStatus({ type: 'error', message: `rosbridge 连接错误: ${error || '未知'}` });
|
|
|
|
|
});
|
|
|
|
|
|
|
|
|
|
ros.on('close', () => {
|
|
|
|
|
const wasConnected = rosConnected;
|
|
|
|
|
rosConnected = false;
|
|
|
|
|
droneConnected = false;
|
|
|
|
|
stopAll();
|
|
|
|
|
if (wasConnected) {
|
|
|
|
|
UIModule.onDroneDisconnected();
|
|
|
|
|
}
|
|
|
|
|
});
|
|
|
|
|
} catch (e) {
|
|
|
|
|
UIModule.onStatus({ type: 'error', message: `连接失败: ${e.message || e}` });
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// ---- Topic subscriptions ----
|
|
|
|
|
|
|
|
|
|
function setupSubscriptions() {
|
|
|
|
|
// Try to detect which namespace has MAVROS
|
|
|
|
|
trySubscribesWithFallback();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Set a default origin for coordinate conversion (no GPS needed)
|
|
|
|
|
function setDefaultOrigin() {
|
|
|
|
|
if (originSet) return;
|
|
|
|
|
// Use map default center as origin - simulation doesn't need real GPS
|
|
|
|
|
origin.lat = MapModule.DEFAULT_LAT || 30.0;
|
|
|
|
|
origin.lng = MapModule.DEFAULT_LNG || 120.0;
|
|
|
|
|
originSet = true;
|
|
|
|
|
originFromGps = false;
|
|
|
|
|
UIModule.onStatus({ type: 'info', message: `仿真模式: 使用默认坐标原点 (${origin.lat}, ${origin.lng})，无需GPS` });
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function trySubscribesWithFallback() {
|
|
|
|
|
let gotState = false;
|
|
|
|
|
const tryNS = uavNS;
|
|
|
|
|
|
|
|
|
|
// Subscribe to MAVROS state
|
|
|
|
|
stateSub = new ROSLIB.Topic({
|
|
|
|
|
ros: ros,
|
|
|
|
|
name: topicName('/mavros/state'),
|
|
|
|
|
messageType: 'mavros_msgs/State'
|
|
|
|
|
});
|
|
|
|
|
|
|
|
|
|
const stateTimeout = setTimeout(() => {
|
|
|
|
|
if (!gotState) {
|
|
|
|
|
// Try without namespace
|
|
|
|
|
if (tryNS !== '') {
|
|
|
|
|
uavNS = '';
|
|
|
|
|
UIModule.onStatus({ type: 'warning', message: `未收到 ${tryNS}/mavros/state，尝试无命名空间...` });
|
|
|
|
|
trySubscribesWithFallback();
|
|
|
|
|
} else {
|
|
|
|
|
UIModule.onStatus({ type: 'error', message: '未检测到 MAVROS 话题，请确认仿真/实机已启动' });
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}, 5000);
|
|
|
|
|
|
|
|
|
|
stateSub.subscribe((msg) => {
|
|
|
|
|
clearTimeout(stateTimeout);
|
|
|
|
|
gotState = true;
|
|
|
|
|
|
|
|
|
|
telemetry.armed = msg.armed || false;
|
|
|
|
|
telemetry.flightMode = msg.mode || 'UNKNOWN';
|
|
|
|
|
|
|
|
|
|
// Check if FCU is truly connected (has valid mode)
|
|
|
|
|
const modeValid = msg.connected && msg.mode && msg.mode.length > 0;
|
|
|
|
|
|
|
|
|
|
if (!droneConnected) {
|
|
|
|
|
droneConnected = true;
|
|
|
|
|
if (modeValid) {
|
|
|
|
|
fcuReady = true;
|
|
|
|
|
UIModule.onStatus({ type: 'success', message: `无人机已连接 (模式: ${msg.mode}, ${msg.armed ? '已解锁' : '未解锁'})` });
|
|
|
|
|
} else {
|
|
|
|
|
UIModule.onStatus({ type: 'warning', message: `MAVROS 已连接，但飞控未就绪 (mode=空)。等待 PX4 初始化...` });
|
|
|
|
|
}
|
|
|
|
|
setDefaultOrigin();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Detect when FCU becomes ready
|
|
|
|
|
if (!fcuReady && modeValid) {
|
|
|
|
|
fcuReady = true;
|
|
|
|
|
UIModule.onStatus({ type: 'success', message: `飞控已就绪! 当前模式: ${msg.mode}, 状态: ${msg.armed ? '已解锁' : '未解锁'}` });
|
|
|
|
|
}
|
|
|
|
|
});
|
|
|
|
|
|
|
|
|
|
// Local position (ENU: x=East, y=North, z=Up)
|
|
|
|
|
localPosSub = new ROSLIB.Topic({
|
|
|
|
|
ros: ros,
|
|
|
|
|
name: topicName('/mavros/local_position/pose'),
|
|
|
|
|
messageType: 'geometry_msgs/PoseStamped'
|
|
|
|
|
});
|
|
|
|
|
|
|
|
|
|
localPosSub.subscribe((msg) => {
|
|
|
|
|
if (msg.pose && msg.pose.position) {
|
|
|
|
|
telemetry.localPos = {
|
|
|
|
|
x: msg.pose.position.x, // East
|
|
|
|
|
y: msg.pose.position.y, // North
|
|
|
|
|
z: msg.pose.position.z // Up
|
|
|
|
|
};
|
|
|
|
|
}
|
|
|
|
|
if (msg.pose && msg.pose.orientation) {
|
|
|
|
|
const o = msg.pose.orientation;
|
|
|
|
|
telemetry.heading = quatToYawDeg(o.x, o.y, o.z, o.w);
|
|
|
|
|
}
|
|
|
|
|
});
|
|
|
|
|
|
|
|
|
|
// GPS (for lat/lng display and origin)
|
|
|
|
|
gpsSub = new ROSLIB.Topic({
|
|
|
|
|
ros: ros,
|
|
|
|
|
name: topicName('/mavros/global_position/raw/fix'),
|
|
|
|
|
messageType: 'sensor_msgs/NavSatFix'
|
|
|
|
|
});
|
|
|
|
|
|
|
|
|
|
gpsSub.subscribe((msg) => {
|
|
|
|
|
telemetry.lat = msg.latitude || 0;
|
|
|
|
|
telemetry.lng = msg.longitude || 0;
|
|
|
|
|
|
|
|
|
|
if (msg.status) {
|
|
|
|
|
telemetry.gpsStatus = msg.status.status >= 0 ? '3D Fix' : 'No Fix';
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Update origin from real GPS (overrides default origin)
|
|
|
|
|
if (msg.latitude !== 0 && Math.abs(msg.latitude) > 1) {
|
|
|
|
|
if (!originFromGps) {
|
|
|
|
|
origin.lat = msg.latitude;
|
|
|
|
|
origin.lng = msg.longitude;
|
|
|
|
|
originSet = true;
|
|
|
|
|
originFromGps = true;
|
|
|
|
|
UIModule.onStatus({ type: 'info', message: `GPS 坐标已更新: ${origin.lat.toFixed(6)}, ${origin.lng.toFixed(6)}` });
|
|
|
|
|
MapModule.centerOnDrone(msg.latitude, msg.longitude);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
});
|
|
|
|
|
|
|
|
|
|
// Battery
|
|
|
|
|
batterySub = new ROSLIB.Topic({
|
|
|
|
|
ros: ros,
|
|
|
|
|
name: topicName('/mavros/battery'),
|
|
|
|
|
messageType: 'sensor_msgs/BatteryState'
|
|
|
|
|
});
|
|
|
|
|
|
|
|
|
|
batterySub.subscribe((msg) => {
|
|
|
|
|
telemetry.battery = (msg.percentage != null) ? msg.percentage * 100 : 100;
|
|
|
|
|
});
|
|
|
|
|
|
|
|
|
|
// Setpoint publisher — PoseStamped is the standard MAVROS position control interface
|
|
|
|
|
setpointPub = new ROSLIB.Topic({
|
|
|
|
|
ros: ros,
|
|
|
|
|
name: topicName('/mavros/setpoint_position/local'),
|
|
|
|
|
messageType: 'geometry_msgs/PoseStamped'
|
|
|
|
|
});
|
|
|
|
|
|
|
|
|
|
// Start throttled telemetry loop (5Hz, decoupled from subscription rate)
|
|
|
|
|
startTelemetryLoop();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function pushTelemetry() {
|
|
|
|
|
let displayLat = telemetry.lat;
|
|
|
|
|
let displayLng = telemetry.lng;
|
|
|
|
|
if ((!displayLat || displayLat === 0) && originSet) {
|
|
|
|
|
const ll = enuToLatLng(telemetry.localPos.x, telemetry.localPos.y);
|
|
|
|
|
displayLat = ll.lat;
|
|
|
|
|
displayLng = ll.lng;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
UIModule.onTelemetry({
|
|
|
|
|
type: 'telemetry',
|
|
|
|
|
lat: displayLat,
|
|
|
|
|
lng: displayLng,
|
|
|
|
|
alt: telemetry.localPos.z,
|
|
|
|
|
speed: telemetry.speed,
|
|
|
|
|
battery: telemetry.battery,
|
|
|
|
|
heading: telemetry.heading,
|
|
|
|
|
flight_mode: telemetry.flightMode,
|
|
|
|
|
gps_status: telemetry.gpsStatus,
|
|
|
|
|
current_wp: (currentWPIndex + 1) + '/' + navWaypoints.length
|
|
|
|
|
});
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function startTelemetryLoop() {
|
|
|
|
|
if (telemetryTimer) clearInterval(telemetryTimer);
|
|
|
|
|
pushTelemetry();
|
|
|
|
|
telemetryTimer = setInterval(pushTelemetry, 200); // 5Hz UI update
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function stopTelemetryLoop() {
|
|
|
|
|
if (telemetryTimer) { clearInterval(telemetryTimer); telemetryTimer = null; }
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// ---- Setpoint publishing ----
|
|
|
|
|
|
|
|
|
|
let spSeq = 0;
|
|
|
|
|
|
|
|
|
|
function publishSetpoint(east, north, up) {
|
|
|
|
|
if (!setpointPub || !rosConnected) return;
|
|
|
|
|
|
|
|
|
|
const msg = new ROSLIB.Message({
|
|
|
|
|
header: {
|
|
|
|
|
seq: spSeq++,
|
|
|
|
|
stamp: { secs: Math.floor(Date.now() / 1000), nsecs: 0 },
|
|
|
|
|
frame_id: 'map'
|
|
|
|
|
},
|
|
|
|
|
pose: {
|
|
|
|
|
position: { x: east, y: north, z: up },
|
|
|
|
|
orientation: { x: 0, y: 0, z: 0, w: 1 }
|
|
|
|
|
}
|
|
|
|
|
});
|
|
|
|
|
|
|
|
|
|
setpointPub.publish(msg);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function startPublishLoop(east, north, up) {
|
|
|
|
|
stopPublishing();
|
|
|
|
|
publishSetpoint(east, north, up);
|
|
|
|
|
publishTimer = setInterval(() => {
|
|
|
|
|
publishSetpoint(east, north, up);
|
|
|
|
|
}, PUBLISH_INTERVAL);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function stopPublishing() {
|
|
|
|
|
if (publishTimer) { clearInterval(publishTimer); publishTimer = null; }
|
|
|
|
|
if (wpCheckTimer) { clearInterval(wpCheckTimer); wpCheckTimer = null; }
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function stopAll() {
|
|
|
|
|
stopPublishing();
|
|
|
|
|
stopTelemetryLoop();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// ---- ROS Service calls ----
|
|
|
|
|
|
|
|
|
|
// Simple one-shot versions for arm/disarm/land buttons
|
|
|
|
|
|
|
|
|
|
// ---- Mission operations ----
|
|
|
|
|
|
|
|
|
|
function uploadMission(waypoints) {
|
|
|
|
|
if (waypoints.length === 0) {
|
|
|
|
|
UIModule.onStatus({ type: 'error', message: '航点列表为空' });
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Store raw waypoints
|
|
|
|
|
rawWaypoints = waypoints;
|
|
|
|
|
currentWPIndex = 0;
|
|
|
|
|
|
|
|
|
|
// Convert to ENU (origin should always be set by now)
|
|
|
|
|
if (originSet) {
|
|
|
|
|
convertWaypoints();
|
|
|
|
|
UIModule.onStatus({ type: 'success', message: `任务上传成功，共 ${navWaypoints.length} 个航点` });
|
|
|
|
|
} else {
|
|
|
|
|
// Fallback (shouldn't happen, but just in case)
|
|
|
|
|
UIModule.onStatus({ type: 'success', message: `已暂存 ${waypoints.length} 个航点` });
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function convertWaypoints() {
|
|
|
|
|
if (!originSet || !rawWaypoints || rawWaypoints.length === 0) return false;
|
|
|
|
|
navWaypoints = rawWaypoints.map(wp => {
|
|
|
|
|
const enu = latLngToENU(wp.lat, wp.lng, wp.alt);
|
|
|
|
|
return { ...enu, hold_time: wp.hold_time || 0 };
|
|
|
|
|
});
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function startMission() {
|
|
|
|
|
if (!rosConnected || !droneConnected) {
|
|
|
|
|
UIModule.onStatus({ type: 'error', message: '未连接无人机' });
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
if (!fcuReady) {
|
|
|
|
|
UIModule.onStatus({ type: 'error', message: '飞控未就绪 (MAVROS 模式为空)。请等待 PX4 完全启动后重试。也可尝试在终端执行: rosservice call ' + serviceName('/mavros/set_mode') + ' "{base_mode: 0, custom_mode: \'OFFBOARD\'}" 验证模式切换是否正常' });
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
if (rawWaypoints.length === 0) {
|
|
|
|
|
UIModule.onStatus({ type: 'error', message: '请先上传任务' });
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Convert waypoints
|
|
|
|
|
if (navWaypoints.length === 0 && rawWaypoints.length > 0) {
|
|
|
|
|
if (!originSet) setDefaultOrigin();
|
|
|
|
|
convertWaypoints();
|
|
|
|
|
}
|
|
|
|
|
if (navWaypoints.length === 0) {
|
|
|
|
|
UIModule.onStatus({ type: 'error', message: '航点转换失败，请重新上传任务' });
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Step 1: Start sending takeoff setpoints at target altitude
|
|
|
|
|
// PX4 requires setpoints BEFORE switching OFFBOARD, and altitude must be positive
|
|
|
|
|
const firstWP = navWaypoints[0];
|
|
|
|
|
const startX = telemetry.localPos.x || 0;
|
|
|
|
|
const startY = telemetry.localPos.y || 0;
|
|
|
|
|
const takeoffAlt = firstWP.z;
|
|
|
|
|
startPublishLoop(startX, startY, takeoffAlt);
|
|
|
|
|
UIModule.onStatus({ type: 'info', message: '正在发送位置设定值 (10Hz)...' });
|
|
|
|
|
|
|
|
|
|
// Step 2: Wait for setpoints to flow, then switch OFFBOARD
|
|
|
|
|
setTimeout(() => {
|
|
|
|
|
callSetModeWithRetry('OFFBOARD', 3, (ok) => {
|
|
|
|
|
if (!ok) {
|
|
|
|
|
stopPublishing();
|
|
|
|
|
UIModule.onStatus({ type: 'error', message: 'OFFBOARD 模式切换失败，任务中止。请在终端验证: rosservice call ' + serviceName('/mavros/set_mode') + ' "{base_mode: 0, custom_mode: \'OFFBOARD\'}"' });
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Step 3: Arm
|
|
|
|
|
setTimeout(() => {
|
|
|
|
|
callArmingWithRetry(true, 3, (ok) => {
|
|
|
|
|
if (!ok) {
|
|
|
|
|
stopPublishing();
|
|
|
|
|
UIModule.onStatus({ type: 'error', message: '电机解锁失败，任务中止。请检查: 1) 传感器是否校准 2) GPS是否定位 3) 安全检查是否通过' });
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Step 4: Wait for takeoff, then navigate
|
|
|
|
|
setTimeout(() => {
|
|
|
|
|
UIModule.onStatus({ type: 'info', message: '正在起飞...' });
|
|
|
|
|
|
|
|
|
|
// Check if drone reached takeoff altitude
|
|
|
|
|
let takeoffDone = false;
|
|
|
|
|
const takeoffCheck = setInterval(() => {
|
|
|
|
|
const curZ = telemetry.localPos.z || 0;
|
|
|
|
|
if (curZ > takeoffAlt - 1.0 && curZ > 0.5) {
|
|
|
|
|
clearInterval(takeoffCheck);
|
|
|
|
|
takeoffDone = true;
|
|
|
|
|
navActive = true;
|
|
|
|
|
navPaused = false;
|
|
|
|
|
UIModule.onStatus({ type: 'success', message: '起飞完成，开始巡航' });
|
|
|
|
|
navigateToWaypoint(0);
|
|
|
|
|
}
|
|
|
|
|
}, 500);
|
|
|
|
|
|
|
|
|
|
// Fallback: start navigation after 15s regardless
|
|
|
|
|
setTimeout(() => {
|
|
|
|
|
clearInterval(takeoffCheck);
|
|
|
|
|
if (!takeoffDone) {
|
|
|
|
|
navActive = true;
|
|
|
|
|
navPaused = false;
|
|
|
|
|
UIModule.onStatus({ type: 'info', message: '任务已开始执行' });
|
|
|
|
|
navigateToWaypoint(0);
|
|
|
|
|
}
|
|
|
|
|
}, 15000);
|
|
|
|
|
}, 1000);
|
|
|
|
|
});
|
|
|
|
|
}, 500);
|
|
|
|
|
});
|
|
|
|
|
}, 3000);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Retry wrappers for service calls
|
|
|
|
|
function callSetModeWithRetry(mode, retries, callback) {
|
|
|
|
|
UIModule.onStatus({ type: 'info', message: `正在切换 ${mode} 模式...` });
|
|
|
|
|
const service = new ROSLIB.Service({
|
|
|
|
|
ros: ros,
|
|
|
|
|
name: serviceName('/mavros/set_mode'),
|
|
|
|
|
serviceType: 'mavros_msgs/SetMode'
|
|
|
|
|
});
|
|
|
|
|
service.callService(
|
|
|
|
|
new ROSLIB.ServiceRequest({ base_mode: 0, custom_mode: mode }),
|
|
|
|
|
(result) => {
|
|
|
|
|
if (result && result.mode_sent) {
|
|
|
|
|
UIModule.onStatus({ type: 'success', message: `模式已切换: ${mode}` });
|
|
|
|
|
callback(true);
|
|
|
|
|
} else if (retries > 0) {
|
|
|
|
|
UIModule.onStatus({ type: 'warning', message: `模式切换未成功，剩余重试 ${retries} 次...` });
|
|
|
|
|
setTimeout(() => callSetModeWithRetry(mode, retries - 1, callback), 2000);
|
|
|
|
|
} else {
|
|
|
|
|
UIModule.onStatus({ type: 'error', message: `模式切换失败: ${JSON.stringify(result)}` });
|
|
|
|
|
callback(false);
|
|
|
|
|
}
|
|
|
|
|
},
|
|
|
|
|
(error) => {
|
|
|
|
|
if (retries > 0) {
|
|
|
|
|
UIModule.onStatus({ type: 'warning', message: `模式切换服务错误，重试中...` });
|
|
|
|
|
setTimeout(() => callSetModeWithRetry(mode, retries - 1, callback), 2000);
|
|
|
|
|
} else {
|
|
|
|
|
UIModule.onStatus({ type: 'error', message: `模式切换服务失败: ${error || '不可用'}` });
|
|
|
|
|
callback(false);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function callArmingWithRetry(arm, retries, callback) {
|
|
|
|
|
UIModule.onStatus({ type: 'info', message: arm ? '正在解锁电机...' : '正在上锁电机...' });
|
|
|
|
|
const service = new ROSLIB.Service({
|
|
|
|
|
ros: ros,
|
|
|
|
|
name: serviceName('/mavros/cmd/arming'),
|
|
|
|
|
serviceType: 'mavros_msgs/CommandBool'
|
|
|
|
|
});
|
|
|
|
|
service.callService(
|
|
|
|
|
new ROSLIB.ServiceRequest({ value: arm }),
|
|
|
|
|
(result) => {
|
|
|
|
|
if (result && result.success) {
|
|
|
|
|
UIModule.onStatus({ type: 'success', message: arm ? '电机已解锁' : '电机已上锁' });
|
|
|
|
|
callback(true);
|
|
|
|
|
} else if (retries > 0) {
|
|
|
|
|
UIModule.onStatus({ type: 'warning', message: `${arm ? '解锁' : '上锁'}未成功 (result=${result.result})，剩余重试 ${retries} 次...` });
|
|
|
|
|
setTimeout(() => callArmingWithRetry(arm, retries - 1, callback), 2000);
|
|
|
|
|
} else {
|
|
|
|
|
UIModule.onStatus({ type: 'error', message: `${arm ? '解锁' : '上锁'}失败: result=${result ? result.result : 'N/A'}` });
|
|
|
|
|
callback(false);
|
|
|
|
|
}
|
|
|
|
|
},
|
|
|
|
|
(error) => {
|
|
|
|
|
if (retries > 0) {
|
|
|
|
|
UIModule.onStatus({ type: 'warning', message: `${arm ? '解锁' : '上锁'}服务错误，重试中...` });
|
|
|
|
|
setTimeout(() => callArmingWithRetry(arm, retries - 1, callback), 2000);
|
|
|
|
|
} else {
|
|
|
|
|
UIModule.onStatus({ type: 'error', message: `${arm ? '解锁' : '上锁'}服务失败` });
|
|
|
|
|
callback(false);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Simple one-shot mode switch (no retry, used by returnHome/land)
|
|
|
|
|
function callSetMode(mode) {
|
|
|
|
|
callSetModeWithRetry(mode, 1, () => {});
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function navigateToWaypoint(index) {
|
|
|
|
|
if (!navActive || navPaused) return;
|
|
|
|
|
|
|
|
|
|
if (index >= navWaypoints.length) {
|
|
|
|
|
UIModule.onStatus({ type: 'success', message: '所有航点已到达，任务完成' });
|
|
|
|
|
const p = telemetry.localPos;
|
|
|
|
|
startPublishLoop(p.x, p.y, p.z);
|
|
|
|
|
navActive = false;
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
currentWPIndex = index;
|
|
|
|
|
const wp = navWaypoints[index];
|
|
|
|
|
|
|
|
|
|
UIModule.onStatus({ type: 'info', message: `飞往航点 ${index + 1}/${navWaypoints.length} (${wp.x.toFixed(1)}, ${wp.y.toFixed(1)}, ${wp.z.toFixed(1)}m)` });
|
|
|
|
|
startPublishLoop(wp.x, wp.y, wp.z);
|
|
|
|
|
checkWaypointReached(index);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function checkWaypointReached(index) {
|
|
|
|
|
if (wpCheckTimer) clearInterval(wpCheckTimer);
|
|
|
|
|
|
|
|
|
|
const wpTimeout = setTimeout(() => {
|
|
|
|
|
// Fallback: if waypoint not reached in 30s, advance anyway
|
|
|
|
|
if (navActive && !navPaused) {
|
|
|
|
|
UIModule.onStatus({ type: 'warning', message: `航点 ${index + 1} 到达超时，跳至下一航点` });
|
|
|
|
|
navigateToWaypoint(index + 1);
|
|
|
|
|
}
|
|
|
|
|
}, 30000);
|
|
|
|
|
|
|
|
|
|
wpCheckTimer = setInterval(() => {
|
|
|
|
|
if (!navActive || navPaused) {
|
|
|
|
|
clearInterval(wpCheckTimer);
|
|
|
|
|
clearTimeout(wpTimeout);
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
const wp = navWaypoints[index];
|
|
|
|
|
if (!wp) { clearTimeout(wpTimeout); return; }
|
|
|
|
|
|
|
|
|
|
const p = telemetry.localPos;
|
|
|
|
|
const dx = p.x - wp.x;
|
|
|
|
|
const dy = p.y - wp.y;
|
|
|
|
|
const dz = p.z - wp.z;
|
|
|
|
|
const dist = Math.sqrt(dx * dx + dy * dy + dz * dz);
|
|
|
|
|
|
|
|
|
|
if (dist < WP_REACH_DIST) {
|
|
|
|
|
clearInterval(wpCheckTimer);
|
|
|
|
|
clearTimeout(wpTimeout);
|
|
|
|
|
wpCheckTimer = null;
|
|
|
|
|
|
|
|
|
|
UIModule.onTelemetry({
|
|
|
|
|
type: 'mission_progress',
|
|
|
|
|
current: index + 1,
|
|
|
|
|
total: navWaypoints.length
|
|
|
|
|
});
|
|
|
|
|
|
|
|
|
|
const holdTime = wp.hold_time || 0;
|
|
|
|
|
if (holdTime > 0) {
|
|
|
|
|
UIModule.onStatus({ type: 'info', message: `航点 ${index + 1} 已到达，悬停 ${holdTime} 秒` });
|
|
|
|
|
setTimeout(() => {
|
|
|
|
|
if (navActive && !navPaused) navigateToWaypoint(index + 1);
|
|
|
|
|
}, holdTime * 1000);
|
|
|
|
|
} else {
|
|
|
|
|
navigateToWaypoint(index + 1);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}, 500);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function pauseMission() {
|
|
|
|
|
if (!navActive || navPaused) return;
|
|
|
|
|
navPaused = true;
|
|
|
|
|
if (wpCheckTimer) { clearInterval(wpCheckTimer); wpCheckTimer = null; }
|
|
|
|
|
const p = telemetry.localPos;
|
|
|
|
|
startPublishLoop(p.x, p.y, p.z);
|
|
|
|
|
UIModule.onStatus({ type: 'info', message: '任务已暂停（悬停中）' });
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function resumeMission() {
|
|
|
|
|
if (!navActive || !navPaused) return;
|
|
|
|
|
navPaused = false;
|
|
|
|
|
navigateToWaypoint(currentWPIndex);
|
|
|
|
|
UIModule.onStatus({ type: 'info', message: '任务已继续' });
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function returnHome() {
|
|
|
|
|
stopPublishing();
|
|
|
|
|
navActive = false;
|
|
|
|
|
navPaused = false;
|
|
|
|
|
|
|
|
|
|
UIModule.onStatus({ type: 'warning', message: '正在返航...' });
|
|
|
|
|
|
|
|
|
|
// Fly to origin at current altitude
|
|
|
|
|
startPublishLoop(0, 0, telemetry.localPos.z);
|
|
|
|
|
|
|
|
|
|
const homeCheck = setInterval(() => {
|
|
|
|
|
const p = telemetry.localPos;
|
|
|
|
|
const dist = Math.sqrt(p.x * p.x + p.y * p.y);
|
|
|
|
|
if (dist < WP_REACH_DIST) {
|
|
|
|
|
clearInterval(homeCheck);
|
|
|
|
|
// Switch to AUTO.LAND for safe landing
|
|
|
|
|
callSetMode('AUTO.LAND');
|
|
|
|
|
setTimeout(() => stopPublishing(), 3000);
|
|
|
|
|
UIModule.onStatus({ type: 'info', message: '已到达起飞点上方，正在降落...' });
|
|
|
|
|
}
|
|
|
|
|
}, 500);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function land() {
|
|
|
|
|
stopPublishing();
|
|
|
|
|
navActive = false;
|
|
|
|
|
navPaused = false;
|
|
|
|
|
// Use PX4 AUTO.LAND mode (safest)
|
|
|
|
|
callSetMode('AUTO.LAND');
|
|
|
|
|
UIModule.onStatus({ type: 'warning', message: '正在降落 (AUTO.LAND)...' });
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function arm() {
|
|
|
|
|
callArmingWithRetry(true, 2, () => {});
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
function disarm() {
|
|
|
|
|
callArmingWithRetry(false, 2, () => {});
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return {
|
|
|
|
|
init,
|
|
|
|
|
isConnected,
|
|
|
|
|
connectDrone,
|
|
|
|
|
uploadMission,
|
|
|
|
|
startMission,
|
|
|
|
|
pauseMission,
|
|
|
|
|
resumeMission,
|
|
|
|
|
returnHome,
|
|
|
|
|
land,
|
|
|
|
|
arm,
|
|
|
|
|
disarm
|
|
|
|
|
};
|
|
|
|
|
})();
|
