/// <summary>
/// Send a state to GCS
/// </summary>
/// <param name="state"></param>
public void SendState(UGVState state)
{
double timestamp = (DateTime.Now - UnixTime).TotalMilliseconds;
Node.SendVehicleGlobalPosition(TargetNodeID, timestamp, NodeID,
(int)(state.Latitude * 10000000.0), (int)(state.Longitude * 10000000.0), 1, 0, 0, 0);
}
/// <summary>
/// Send a state
/// </summary>
/// <param name="state"></param>
public void SendState(UGVState state, byte id = 1)
{
double timestamp = (DateTime.Now - UnixTime).TotalMilliseconds;
//id, longitude, latitude, altitude, x_speed, y_speed, z_speed
NGCP.VehicleGlobalPosition vgp = new NGCP.VehicleGlobalPosition((byte)NodeID, (float)state.Longitude, (float)state.Latitude, 0);
Node.Send(vgp, id);
}
/// <summary>
/// Capture UGV state
/// </summary>
/// <param name="ugv"></param>
/// <returns></returns>
public static UGVState Capture(UGV ugv)
{
UGVState state = new UGVState();
state.Latitude = ugv.Latitude;
state.Longitude = ugv.Longitude;
state.Pitch = ugv.Pitch;
state.Roll = ugv.Roll;
state.Heading = ugv.Heading;
state.FrontWheelOutput = ugv.FinalFrontWheel;
state.RearWheelOutput = ugv.FinalRearWheel;
state.SteerOutput = ugv.FinalSteering;
state.WayPointBearing = ugv.NextWaypointBearing;
state.WayPointDistance = ugv.NextWaypointDistance;
state.DriveMode = (byte)ugv.Settings.DriveMode;
state.TargetLocation = ugv.TargetLockedLocation;
return(state);
}