Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/main'
Browse files Browse the repository at this point in the history
  • Loading branch information
@asvol
asvol committed Dec 18, 2023
2 parents 408e608 + e6aaeef commit 6e38bf0
Show file tree
Hide file tree
Showing 6 changed files with 156 additions and 20 deletions.
46 changes: 46 additions & 0 deletions src/Asv.Mavlink.Test/Microservices/AsvSdr/AsvSdrExTests.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -702,6 +702,8 @@ public async Task Check_System_Control_Action_Reboot_Behaviour()
MavResult.MavResultAccepted),
AsvSdrSystemControlAction.AsvSdrSystemControlActionShutdown => Task.FromResult(
MavResult.MavResultFailed),
AsvSdrSystemControlAction.AsvSdrSystemControlActionRestart => Task.FromResult(
MavResult.MavResultFailed),
_ => Task.FromResult(MavResult.MavResultUnsupported)
};
};
Expand All @@ -713,8 +715,12 @@ await asvSdrClientEx.SystemControlAction(AsvSdrSystemControlAction.AsvSdrSystemC
var mavShutdownResult = await asvSdrClientEx.SystemControlAction(AsvSdrSystemControlAction.AsvSdrSystemControlActionShutdown,
CancellationToken.None);

var mavRestartResult = await asvSdrClientEx.SystemControlAction(AsvSdrSystemControlAction.AsvSdrSystemControlActionRestart,
CancellationToken.None);

Assert.True(mavRebootResult == MavResult.MavResultAccepted);
Assert.True(mavShutdownResult == MavResult.MavResultFailed);
Assert.True(mavRestartResult == MavResult.MavResultFailed);
}

[Fact]
Expand All @@ -730,6 +736,8 @@ public async Task Check_System_Control_Action_Shutdown_Behaviour()
MavResult.MavResultFailed),
AsvSdrSystemControlAction.AsvSdrSystemControlActionShutdown => Task.FromResult(
MavResult.MavResultAccepted),
AsvSdrSystemControlAction.AsvSdrSystemControlActionRestart => Task.FromResult(
MavResult.MavResultFailed),
_ => Task.FromResult(MavResult.MavResultUnsupported)
};
};
Expand All @@ -741,8 +749,46 @@ await asvSdrClientEx.SystemControlAction(AsvSdrSystemControlAction.AsvSdrSystemC
var mavShutdownResult = await asvSdrClientEx.SystemControlAction(AsvSdrSystemControlAction.AsvSdrSystemControlActionShutdown,
CancellationToken.None);

var mavRestartResult = await asvSdrClientEx.SystemControlAction(AsvSdrSystemControlAction.AsvSdrSystemControlActionRestart,
CancellationToken.None);

Assert.True(mavRebootResult == MavResult.MavResultFailed);
Assert.True(mavShutdownResult == MavResult.MavResultAccepted);
Assert.True(mavRestartResult == MavResult.MavResultFailed);
}

[Fact]
public async Task Check_System_Control_Action_Restart_Behaviour()
{
var (asvSdrClientEx, asvSdrServerEx) = await SetUpConnection();

asvSdrServerEx.SystemControlAction = (action, cancel) =>
{
return action switch
{
AsvSdrSystemControlAction.AsvSdrSystemControlActionReboot => Task.FromResult(
MavResult.MavResultFailed),
AsvSdrSystemControlAction.AsvSdrSystemControlActionShutdown => Task.FromResult(
MavResult.MavResultFailed),
AsvSdrSystemControlAction.AsvSdrSystemControlActionRestart => Task.FromResult(
MavResult.MavResultAccepted),
_ => Task.FromResult(MavResult.MavResultUnsupported)
};
};

var mavRebootResult =
await asvSdrClientEx.SystemControlAction(AsvSdrSystemControlAction.AsvSdrSystemControlActionReboot,
CancellationToken.None);

var mavShutdownResult = await asvSdrClientEx.SystemControlAction(AsvSdrSystemControlAction.AsvSdrSystemControlActionShutdown,
CancellationToken.None);

var mavRestartResult = await asvSdrClientEx.SystemControlAction(AsvSdrSystemControlAction.AsvSdrSystemControlActionRestart,
CancellationToken.None);

Assert.True(mavRebootResult == MavResult.MavResultFailed);
Assert.True(mavShutdownResult == MavResult.MavResultFailed);
Assert.True(mavRestartResult == MavResult.MavResultAccepted);
}


Expand Down
107 changes: 87 additions & 20 deletions src/Asv.Mavlink/Microservices/Missions/Client/Ex/IMissionClientEx.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,34 +41,101 @@ public static MissionItem AddSplineMissionItem(this IMissionClientEx vehicle, Ge
item.Param4.OnNext(0);
return item;
}
public static MissionItem AddNavMissionItem(this IMissionClientEx vehicle, GeoPoint point)

/// <summary>
/// Change speed and/or throttle set points. The value persists until it is overridden or there is a mode change
/// </summary>
/// <param name="speed">Speed (-1 indicates no change, -2 indicates return to default vehicle speed)</param>
/// <param name="speedType">Speed type of value set in param2 (such as airspeed, ground speed, and so on)</param>
/// <param name="throttle">Throttle (-1 indicates no change, -2 indicates return to default vehicle throttle value)</param>
/// <returns></returns>
public static MissionItem SetVehicleSpeed(this IMissionClientEx vehicle, float speed, float speedType = 1, float throttle = -1)
{
var item = vehicle.Create();
item.Location.OnNext(point);
item.AutoContinue.OnNext(true);
item.Command.OnNext(MavCmd.MavCmdNavWaypoint);
item.Command.OnNext(MavCmd.MavCmdDoChangeSpeed);
item.Current.OnNext(false);
item.Frame.OnNext(MavFrame.MavFrameGlobalInt);
item.MissionType.OnNext(MavMissionType.MavMissionTypeMission);
item.Param1.OnNext(0);
item.Param2.OnNext(0);
item.Param3.OnNext(0);
item.Param4.OnNext(0);
item.Param1.OnNext(speedType);
item.Param2.OnNext(speed);
item.Param3.OnNext(throttle);
return item;
}
public static MissionItem AddRoiMissionItem(this IMissionClientEx vehicle, GeoPoint point)

/// <summary>
/// Adds navigation waypoint to MissionClients collection
/// </summary>
/// <param name="vehicle">vehicle client</param>
/// <param name="point">item location</param>
/// <param name="holdTime">time in seconds</param>
/// <param name="acceptRadius">radius in meters</param>
/// <param name="passRadius">radius in meters</param>
/// <param name="yawAngle">angle in degrees</param>
/// <returns>MissionItem creation result</returns>
public static MissionItem AddNavMissionItem(this IMissionClientEx vehicle, GeoPoint point,
float holdTime = 0, float acceptRadius = 0, float passRadius = 0, float yawAngle = float.NaN)
{
var item = vehicle.Create();
item.Location.OnNext(point);
item.AutoContinue.OnNext(true);
item.Command.OnNext(MavCmd.MavCmdDoSetRoiLocation);
item.Current.OnNext(false);
item.Frame.OnNext(MavFrame.MavFrameGlobalInt);
item.MissionType.OnNext(MavMissionType.MavMissionTypeMission);
item.Param1.OnNext(0);
item.Param2.OnNext(0);
item.Param3.OnNext(0);
item.Param4.OnNext(0);
return item;
var missionItem = vehicle.Create();
missionItem.Location.OnNext(point);
missionItem.AutoContinue.OnNext(true);
missionItem.Command.OnNext(MavCmd.MavCmdNavWaypoint);
missionItem.Current.OnNext(false);
missionItem.Frame.OnNext(MavFrame.MavFrameGlobalInt);
missionItem.MissionType.OnNext(MavMissionType.MavMissionTypeMission);
missionItem.Param1.OnNext(holdTime);
missionItem.Param2.OnNext(acceptRadius);
missionItem.Param3.OnNext(passRadius);
missionItem.Param4.OnNext(yawAngle);
return missionItem;
}

public static MissionItem AddTakeOffMissionItem(this IMissionClientEx vehicle, GeoPoint point, float pitch = 0, float yawAngle = float.NaN)
{
var missionItem = vehicle.Create();
missionItem.Location.OnNext(point);
missionItem.AutoContinue.OnNext(true);
missionItem.Command.OnNext(MavCmd.MavCmdNavTakeoff);
missionItem.Current.OnNext(false);
missionItem.Frame.OnNext(MavFrame.MavFrameGlobalInt);
missionItem.MissionType.OnNext(MavMissionType.MavMissionTypeMission);
missionItem.Param1.OnNext(pitch);
missionItem.Param2.OnNext(0.0f);
missionItem.Param3.OnNext(0.0f);
missionItem.Param4.OnNext(yawAngle);
return missionItem;
}

public static MissionItem AddLandMissionItem(this IMissionClientEx vehicle, GeoPoint point, float abortAltitude = 0,
PrecisionLandMode landMode = PrecisionLandMode.PrecisionLandModeDisabled, float yawAngle = float.NaN)
{
var missionItem = vehicle.Create();
missionItem.Location.OnNext(point);
missionItem.AutoContinue.OnNext(true);
missionItem.Command.OnNext(MavCmd.MavCmdNavLand);
missionItem.Current.OnNext(false);
missionItem.Frame.OnNext(MavFrame.MavFrameGlobalInt);
missionItem.MissionType.OnNext(MavMissionType.MavMissionTypeMission);
missionItem.Param1.OnNext(abortAltitude);
missionItem.Param2.OnNext((float)landMode);
missionItem.Param3.OnNext(0.0f);
missionItem.Param4.OnNext(yawAngle);
return missionItem;
}

public static MissionItem AddRoiMissionItem(this IMissionClientEx vehicle, GeoPoint point, MavRoi roiMode = MavRoi.MavRoiLocation, float wpIndex = 0, float roiIndex = 0)
{
var missionItem = vehicle.Create();
missionItem.Location.OnNext(point);
missionItem.AutoContinue.OnNext(true);
missionItem.Command.OnNext(MavCmd.MavCmdDoSetRoi);
missionItem.Current.OnNext(false);
missionItem.Frame.OnNext(MavFrame.MavFrameGlobalInt);
missionItem.MissionType.OnNext(MavMissionType.MavMissionTypeMission);
missionItem.Param1.OnNext((float)roiMode);
missionItem.Param2.OnNext(wpIndex);
missionItem.Param3.OnNext(roiIndex);
missionItem.Param4.OnNext(0.0f);
return missionItem;
}
}
3 changes: 3 additions & 0 deletions src/Asv.Mavlink/Microservices/Missions/Server/Ex/IMissionServerEx.cs
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
using System;
using System.Collections.Generic;
using Asv.Common;
using DynamicData;

Expand All @@ -10,4 +11,6 @@ public interface IMissionServerEx
IRxEditableValue<ushort> Current { get; }
IRxEditableValue<ushort> Reached { get; }
IObservable<IChangeSet<ServerMissionItem,ushort>> Items { get; }
void AddItems(IEnumerable<ServerMissionItem> items);
void RemoveItems(IEnumerable<ServerMissionItem> items);
}
12 changes: 12 additions & 0 deletions src/Asv.Mavlink/Microservices/Missions/Server/Ex/MissionServerEx.cs
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
using System;
using System.Collections.Generic;
using System.Reactive.Concurrency;
using System.Threading;
using Asv.Common;
Expand Down Expand Up @@ -121,5 +122,16 @@ private async void UploadMission(MissionCountPacket req)
public IRxEditableValue<ushort> Current { get; }
public IRxEditableValue<ushort> Reached { get; }
public IObservable<IChangeSet<ServerMissionItem, ushort>> Items => _missionSource.Connect();
public void AddItems(IEnumerable<ServerMissionItem> items)
{
if(items != null)
_missionSource.AddOrUpdate(items);
}

public void RemoveItems(IEnumerable<ServerMissionItem> items)
{
if(items != null)
_missionSource.Remove(items);
}
}

3 changes: 3 additions & 0 deletions src/Asv.Mavlink/Protocol/Dialects/asv_sdr.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -217,6 +217,9 @@
<entry name="ASV_SDR_SYSTEM_CONTROL_ACTION_SHUTDOWN">
<description>Request system shutdown [!WRAP_TO_V2_EXTENSION_PACKET!]</description>
</entry>
<entry name="ASV_SDR_SYSTEM_CONTROL_ACTION_RESTART">
<description>Request software reboot [!WRAP_TO_V2_EXTENSION_PACKET!]</description>
</entry>
</enum>

<enum name="ASV_SDR_CALIB_STATE">
Expand Down
5 changes: 5 additions & 0 deletions src/Asv.Mavlink/Protocol/Messages/asv_sdr.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -358,6 +358,11 @@ public enum AsvSdrSystemControlAction:uint
/// ASV_SDR_SYSTEM_CONTROL_ACTION_SHUTDOWN
/// </summary>
AsvSdrSystemControlActionShutdown = 1,
/// <summary>
/// Request software restart [!WRAP_TO_V2_EXTENSION_PACKET!]
/// ASV_SDR_SYSTEM_CONTROL_ACTION_RESTART
/// </summary>
AsvSdrSystemControlActionRestart = 2,
}

/// <summary>
Expand Down

0 comments on commit 6e38bf0

Please sign in to comment.