添加了降落任务,一键起飞,测试电机,flyto的下降点,检测所有飞机5秒平均电压,通信模块的测试,起飞延时,降落延时
This commit is contained in:
parent
08d4474798
commit
f0b1a62334
@ -133,6 +133,7 @@ namespace Plane.Copters
|
|||||||
|
|
||||||
Task LEDAsync();
|
Task LEDAsync();
|
||||||
|
|
||||||
|
//电机转动
|
||||||
|
Task MotorTestAsync(int motor);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -195,8 +195,11 @@ namespace Plane.CopterManagement
|
|||||||
return Copter.InjectGpsDataAsync(data, length);
|
return Copter.InjectGpsDataAsync(data, length);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public Task MotorTestAsync(int motor)
|
||||||
|
{
|
||||||
|
return Copter.TakeOffAsync(motor);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
public Task TakeOffAsync(float alt)
|
public Task TakeOffAsync(float alt)
|
||||||
{
|
{
|
||||||
|
@ -260,6 +260,7 @@ namespace Plane.Copters
|
|||||||
req.param4 = mission.Param4;
|
req.param4 = mission.Param4;
|
||||||
|
|
||||||
req.seq = mission.Sequence;
|
req.seq = mission.Sequence;
|
||||||
|
|
||||||
|
|
||||||
return _internalCopter.GeneratePacketAsync(MAVLINK_MSG_ID_MISSION_ITEM, req);
|
return _internalCopter.GeneratePacketAsync(MAVLINK_MSG_ID_MISSION_ITEM, req);
|
||||||
}
|
}
|
||||||
|
@ -121,6 +121,13 @@ namespace Plane.Copters
|
|||||||
return TaskUtils.CompletedTask;
|
return TaskUtils.CompletedTask;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public Task MotorTestAsync(int motor)
|
||||||
|
{
|
||||||
|
return TaskUtils.CompletedTask;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
public Task InjectGpsDataAsync(byte[] data, ushort length)
|
public Task InjectGpsDataAsync(byte[] data, ushort length)
|
||||||
{
|
{
|
||||||
return TaskUtils.CompletedTask;
|
return TaskUtils.CompletedTask;
|
||||||
|
@ -237,6 +237,11 @@ namespace Plane.Copters
|
|||||||
return TaskUtils.CompletedTask;
|
return TaskUtils.CompletedTask;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public Task MotorTestAsync(int motor)
|
||||||
|
{
|
||||||
|
return TaskUtils.CompletedTask;
|
||||||
|
}
|
||||||
|
|
||||||
public override Task SetChannelsAsync()
|
public override Task SetChannelsAsync()
|
||||||
{
|
{
|
||||||
Channel1 = DesiredChannel1 ?? Channel1;
|
Channel1 = DesiredChannel1 ?? Channel1;
|
||||||
|
@ -134,9 +134,10 @@ namespace Plane.Copters
|
|||||||
/// 创建降落任务。
|
/// 创建降落任务。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <returns>降落任务。</returns>
|
/// <returns>降落任务。</returns>
|
||||||
public static IMission CreateLandMission() => new Mission
|
public static IMission CreateLandMission(int waittime) => new Mission
|
||||||
{
|
{
|
||||||
Command = FlightCommand.Land
|
Command = FlightCommand.Land,
|
||||||
|
Param1 = waittime, //停留时间 s
|
||||||
};
|
};
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
@ -148,6 +149,9 @@ namespace Plane.Copters
|
|||||||
Command = FlightCommand.ReturnToLaunch
|
Command = FlightCommand.ReturnToLaunch
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 创建航点任务。
|
/// 创建航点任务。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
|
@ -13,7 +13,7 @@ namespace Plane.Copters
|
|||||||
{
|
{
|
||||||
private double _FlightDistance2D;
|
private double _FlightDistance2D;
|
||||||
|
|
||||||
private PlaneCopter _internalCopter;
|
public PlaneCopter _internalCopter;
|
||||||
|
|
||||||
private int _setModeCount = 0;
|
private int _setModeCount = 0;
|
||||||
|
|
||||||
@ -74,6 +74,11 @@ namespace Plane.Copters
|
|||||||
return await _internalCopter.GetParamAsync(paramName, millisecondsTimeout).ConfigureAwait(false);
|
return await _internalCopter.GetParamAsync(paramName, millisecondsTimeout).ConfigureAwait(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public async Task MotorTestAsync(int motor)
|
||||||
|
{
|
||||||
|
await _internalCopter.DoMotorTestAsync(motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5 , 5).ConfigureAwait(false);
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 锁定飞行器。在调用之前务必判断高度及让使用者确认!
|
/// 锁定飞行器。在调用之前务必判断高度及让使用者确认!
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@ -288,7 +293,7 @@ namespace Plane.Copters
|
|||||||
{
|
{
|
||||||
var currentTakeOffCount = ++_takeOffCount;
|
var currentTakeOffCount = ++_takeOffCount;
|
||||||
// 林俊清, 20160312, 从固件版本 3.1.1(0x3101)开始支持直接解锁起飞命令。
|
// 林俊清, 20160312, 从固件版本 3.1.1(0x3101)开始支持直接解锁起飞命令。
|
||||||
if (FirmwareVersion >= 0x3101)
|
//if (FirmwareVersion >= 0x3101)
|
||||||
{
|
{
|
||||||
await SetChannelsAsync(
|
await SetChannelsAsync(
|
||||||
ch1: 1500,
|
ch1: 1500,
|
||||||
@ -315,6 +320,7 @@ namespace Plane.Copters
|
|||||||
}
|
}
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
/*
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
await UnlockAsync().ConfigureAwait(false);
|
await UnlockAsync().ConfigureAwait(false);
|
||||||
@ -329,6 +335,7 @@ namespace Plane.Copters
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
*/
|
||||||
}
|
}
|
||||||
|
|
||||||
public async Task UnlockAsync()
|
public async Task UnlockAsync()
|
||||||
|
@ -714,7 +714,7 @@ namespace Plane.Copters
|
|||||||
|
|
||||||
public async Task<bool> DoARMAsync(bool armit)
|
public async Task<bool> DoARMAsync(bool armit)
|
||||||
{
|
{
|
||||||
return await DoCommandAsync(MAVLink.MAV_CMD.COMPONENT_ARM_DISARM, armit ? 1 : 0, 0, 0, 0, 0, 0, 0);
|
return await DoCommandAsync(MAVLink.MAV_CMD.COMPONENT_ARM_DISARM, armit ? 1 : 0, 21196, 0, 0, 0, 0, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
public async Task<bool> DoMotorTestAsync(int motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout)
|
public async Task<bool> DoMotorTestAsync(int motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout)
|
||||||
@ -749,6 +749,8 @@ namespace Plane.Copters
|
|||||||
|
|
||||||
req.command = (ushort)actionid;
|
req.command = (ushort)actionid;
|
||||||
|
|
||||||
|
|
||||||
|
Console.WriteLine("P1 = " + p1);
|
||||||
req.param1 = p1;
|
req.param1 = p1;
|
||||||
req.param2 = p2;
|
req.param2 = p2;
|
||||||
req.param3 = p3;
|
req.param3 = p3;
|
||||||
|
@ -9,6 +9,7 @@
|
|||||||
<Import_RootNamespace>Plane</Import_RootNamespace>
|
<Import_RootNamespace>Plane</Import_RootNamespace>
|
||||||
</PropertyGroup>
|
</PropertyGroup>
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
|
<Compile Include="$(MSBuildThisFileDirectory)CommunicationManagement\CommModule.cs" />
|
||||||
<Compile Include="$(MSBuildThisFileDirectory)Communication\ExceptionThrownEventSource.cs" />
|
<Compile Include="$(MSBuildThisFileDirectory)Communication\ExceptionThrownEventSource.cs" />
|
||||||
<Compile Include="$(MSBuildThisFileDirectory)Communication\EmptyConnection.cs" />
|
<Compile Include="$(MSBuildThisFileDirectory)Communication\EmptyConnection.cs" />
|
||||||
<Compile Include="$(MSBuildThisFileDirectory)Communication\CompositeConnection.cs" />
|
<Compile Include="$(MSBuildThisFileDirectory)Communication\CompositeConnection.cs" />
|
||||||
@ -45,6 +46,7 @@
|
|||||||
<Compile Include="$(MSBuildThisFileDirectory)Copters\FakeCopter.cs" />
|
<Compile Include="$(MSBuildThisFileDirectory)Copters\FakeCopter.cs" />
|
||||||
<Compile Include="$(MSBuildThisFileDirectory)Copters\FlightMode.cs" />
|
<Compile Include="$(MSBuildThisFileDirectory)Copters\FlightMode.cs" />
|
||||||
<Compile Include="$(MSBuildThisFileDirectory)Copters\Mission.cs" />
|
<Compile Include="$(MSBuildThisFileDirectory)Copters\Mission.cs" />
|
||||||
|
<Compile Include="$(MSBuildThisFileDirectory)Protocols\MavCommTypes.cs" />
|
||||||
<Compile Include="$(MSBuildThisFileDirectory)Protocols\MavlinkCRC.cs" />
|
<Compile Include="$(MSBuildThisFileDirectory)Protocols\MavlinkCRC.cs" />
|
||||||
<Compile Include="$(MSBuildThisFileDirectory)Protocols\MAVLinkTypes.cs" />
|
<Compile Include="$(MSBuildThisFileDirectory)Protocols\MAVLinkTypes.cs" />
|
||||||
<Compile Include="$(MSBuildThisFileDirectory)Protocols\MavlinkUtil_NET45.cs" />
|
<Compile Include="$(MSBuildThisFileDirectory)Protocols\MavlinkUtil_NET45.cs" />
|
||||||
|
Loading…
Reference in New Issue
Block a user