public void SendPWMValue(int PWMValue)
{
// Send read request
byte[] toSend = Comm.GetBytes(PWMValue);
formMain.SendData(0x20, toSend);
}
private void buttonWriteParams_Click(object sender, EventArgs e)
{
// Get data from form
SParameters p = new SParameters();
p.GyroOffX = float.Parse(textBoxParam1.Text);
p.GyroOffY = float.Parse(textBoxParam2.Text);
p.GyroOffZ = float.Parse(textBoxParam3.Text);
p.MagOffX = float.Parse(textBoxParam4.Text);
p.MagOffY = float.Parse(textBoxParam5.Text);
p.MagOffZ = float.Parse(textBoxParam6.Text);
p.AttOffRoll = float.Parse(textBoxParam7.Text);
p.AttOffPitch = float.Parse(textBoxParam8.Text);
p.RollMax = float.Parse(textBoxParam9.Text);
p.RollKp = float.Parse(textBoxParam10.Text);
p.RollKi = float.Parse(textBoxParam11.Text);
p.RollKd = float.Parse(textBoxParam12.Text);
p.PitchMax = float.Parse(textBoxParam13.Text);
p.PitchKp = float.Parse(textBoxParam14.Text);
p.PitchKi = float.Parse(textBoxParam15.Text);
p.PitchKd = float.Parse(textBoxParam16.Text);
// Add CRC
byte[] data = Comm.GetBytes(p);
p.CRC32 = Crc32.CalculateCrc32(data, data.Length - sizeof(uint));
mainSystem.WriteParams(p);
ParamsCommandAckCntExpected = ParamsCommandAckCntReceived + 1;
}
public void AbortWaypoints()
{
SCommGotoExecute executeWaypointsCmd = new SCommGotoExecute();
executeWaypointsCmd.Command = 10; // abort trajectory
// Send
byte[] toSend = Comm.GetBytes(executeWaypointsCmd);
formMain.SendData(0x80, toSend);
}
public void ExecuteWaypoints(float velocity)
{
SCommGotoExecute executeWaypointsCmd = new SCommGotoExecute();
executeWaypointsCmd.Command = 0x02; // execute trajectory
executeWaypointsCmd.Velocity = velocity;
// Send
byte[] toSend = Comm.GetBytes(executeWaypointsCmd);
formMain.SendData(0x80, toSend);
}
private void Goto(SWaypoint targetWaypoint)
{
SCommGotoExecute gotoExecuteCmd = new SCommGotoExecute();
gotoExecuteCmd.Command = 0x01; // execute GOTO
gotoExecuteCmd.TargetWaypoint.Altitude = targetWaypoint.Altitude;
gotoExecuteCmd.TargetWaypoint.Longitude = (int)(targetWaypoint.Longitude * 1e7);
gotoExecuteCmd.TargetWaypoint.Latitude = (int)(targetWaypoint.Latitude * 1e7);
// Send
byte[] toSend = Comm.GetBytes(gotoExecuteCmd);
formMain.SendData(0x80, toSend);
}
public void ExecuteOrbit(SWaypoint targetCenter, float velocity)
{
SCommGotoExecute executeOrbitCmd = new SCommGotoExecute();
executeOrbitCmd.Command = 0x03; // execute orbit
executeOrbitCmd.TargetWaypoint.Altitude = targetCenter.Altitude;
executeOrbitCmd.TargetWaypoint.Longitude = (int)(targetCenter.Longitude * 1e7);
executeOrbitCmd.TargetWaypoint.Latitude = (int)(targetCenter.Latitude * 1e7);
executeOrbitCmd.Velocity = velocity;
// Send
byte[] toSend = Comm.GetBytes(executeOrbitCmd);
formMain.SendData(0x80, toSend);
}
public void DownloadWaypoints()
{
SCommWaypoints wayPoints = new SCommWaypoints();
wayPoints.waypoints = new SWpt[8];
wayPoints.WaypointCnt = (uint)Waypoints.Count;
for (int i = 0; i != Waypoints.Count; i++)
{
wayPoints.waypoints[i].Altitude = Waypoints[i].Altitude;
wayPoints.waypoints[i].Longitude = (int)(Waypoints[i].Longitude * 1e7);
wayPoints.waypoints[i].Latitude = (int)(Waypoints[i].Latitude * 1e7);
}
// Send
byte[] toSend = Comm.GetBytes(wayPoints);
formMain.SendData(0x81, toSend);
}
public void WpnCommand(byte index, byte command)
{
uint code = 0x43782843;
uint timer = 400; // ticks, 1 sec
SCommLaunch launch = new SCommLaunch();
launch.Command = command;
launch.Index = index;
if (command == 2)
{
launch.CodeTimer = timer;
}
else
{
launch.CodeTimer = code;
}
// Send
byte[] toSend = Comm.GetBytes(launch);
SendData(0x90, toSend);
}
public void WriteParams(SParameters p)
{
// Send
byte[] toSend = Comm.GetBytes(p);
formMain.SendData(0x60, toSend);
}
