Skip to content

Commit

Permalink
rad2deg and deg2rad: convert to double
Browse files Browse the repository at this point in the history
  • Loading branch information
meee1 committed Feb 22, 2017
1 parent e1a4e7c commit 3b3619b
Show file tree
Hide file tree
Showing 27 changed files with 9,061 additions and 9,061 deletions.
2,516 changes: 1,258 additions & 1,258 deletions Common.cs

Large diffs are not rendered by default.

12 changes: 6 additions & 6 deletions CurrentState.cs
Original file line number Diff line number Diff line change
Expand Up @@ -834,7 +834,7 @@ public float ELToMAV

float altdiff = (float) (_altasl - TrackerLocation.Alt);

float angle = (float) Math.Atan(altdiff/dist)*rad2deg;
float angle = (float) (Math.Atan(altdiff/dist)*rad2deg);

return angle;
}
Expand Down Expand Up @@ -1110,8 +1110,8 @@ public void ResetInternals()
}
}

const float rad2deg = (float) (180/Math.PI);
const float deg2rad = (float) (1.0/rad2deg);
const double rad2deg = (float) (180/Math.PI);
const double deg2rad = (float) (1.0/rad2deg);

private DateTime lastupdate = DateTime.Now;

Expand Down Expand Up @@ -1749,9 +1749,9 @@ public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool up
{
var att = mavLinkMessage.ToStructure<MAVLink.mavlink_attitude_t>();

roll = att.roll*rad2deg;
pitch = att.pitch*rad2deg;
yaw = att.yaw*rad2deg;
roll = (float)(att.roll*rad2deg);
pitch = (float)(att.pitch*rad2deg);
yaw = (float)(att.yaw*rad2deg);

//Console.WriteLine(MAV.sysid + " " +roll + " " + pitch + " " + yaw);

Expand Down
156 changes: 78 additions & 78 deletions GCSViews/ConfigurationView/ConfigHWAirspeed.cs
Original file line number Diff line number Diff line change
@@ -1,79 +1,79 @@
using System;
using System.Collections.Generic;
using System.Windows.Forms;
using MissionPlanner.Controls;

namespace MissionPlanner.GCSViews.ConfigurationView
{
public partial class ConfigHWAirspeed : UserControl, IActivate
{
private const float rad2deg = (float) (180/Math.PI);
private const float deg2rad = (float) (1.0/rad2deg);
private bool startup;

public ConfigHWAirspeed()
{
InitializeComponent();
}

public void Activate()
{
if (!MainV2.comPort.BaseStream.IsOpen)
{
Enabled = false;
return;
}
Enabled = true;

startup = true;


CHK_airspeeduse.setup(1, 0, "ARSPD_USE", MainV2.comPort.MAV.param);
CHK_enableairspeed.setup(1, 0, "ARSPD_ENABLE", MainV2.comPort.MAV.param);

var options = new List<KeyValuePair<int, string>>();
options.Add(new KeyValuePair<int, string>(0, "APM 2 analog pin 0"));
options.Add(new KeyValuePair<int, string>(1, "APM 2 analog pin 1"));
options.Add(new KeyValuePair<int, string>(2, "APM 2 analog pin 2"));
options.Add(new KeyValuePair<int, string>(3, "APM 2 analog pin 3"));
options.Add(new KeyValuePair<int, string>(4, "APM 2 analog pin 4"));
options.Add(new KeyValuePair<int, string>(5, "APM 2 analog pin 5"));
options.Add(new KeyValuePair<int, string>(6, "APM 2 analog pin 6"));
options.Add(new KeyValuePair<int, string>(7, "APM 2 analog pin 7"));
options.Add(new KeyValuePair<int, string>(8, "APM 2 analog pin 8"));
options.Add(new KeyValuePair<int, string>(9, "APM 2 analog pin 9"));

options.Add(new KeyValuePair<int, string>(64, "APM 1 AS Port"));

options.Add(new KeyValuePair<int, string>(11, "PX4 Analog AS Port"));
options.Add(new KeyValuePair<int, string>(15, "Pixhawk Analog AS Port"));
options.Add(new KeyValuePair<int, string>(65, "PX4/Pixhawk EagleTree or MEAS I2C AS Sensor"));

mavlinkCheckBoxAirspeed_pin.setup(options, "ARSPD_PIN", MainV2.comPort.MAV.param);


startup = false;
}

private void CHK_enableairspeed_CheckedChanged(object sender, EventArgs e)
{
if (startup)
return;
try
{
if (MainV2.comPort.MAV.param["ARSPD_ENABLE"] == null)
{
CustomMessageBox.Show(Strings.ErrorFeatureNotEnabled, Strings.ERROR);
}
else
{
MainV2.comPort.setParam("ARSPD_ENABLE", ((CheckBox) sender).Checked ? 1 : 0);
}
}
catch
{
CustomMessageBox.Show(string.Format(Strings.ErrorSetValueFailed, "ARSPD_ENABLE"), Strings.ERROR);
}
}
}
using System;
using System.Collections.Generic;
using System.Windows.Forms;
using MissionPlanner.Controls;

namespace MissionPlanner.GCSViews.ConfigurationView
{
public partial class ConfigHWAirspeed : UserControl, IActivate
{
private const double rad2deg = (float) (180/Math.PI);
private const double deg2rad = (float) (1.0/rad2deg);
private bool startup;

public ConfigHWAirspeed()
{
InitializeComponent();
}

public void Activate()
{
if (!MainV2.comPort.BaseStream.IsOpen)
{
Enabled = false;
return;
}
Enabled = true;

startup = true;


CHK_airspeeduse.setup(1, 0, "ARSPD_USE", MainV2.comPort.MAV.param);
CHK_enableairspeed.setup(1, 0, "ARSPD_ENABLE", MainV2.comPort.MAV.param);

var options = new List<KeyValuePair<int, string>>();
options.Add(new KeyValuePair<int, string>(0, "APM 2 analog pin 0"));
options.Add(new KeyValuePair<int, string>(1, "APM 2 analog pin 1"));
options.Add(new KeyValuePair<int, string>(2, "APM 2 analog pin 2"));
options.Add(new KeyValuePair<int, string>(3, "APM 2 analog pin 3"));
options.Add(new KeyValuePair<int, string>(4, "APM 2 analog pin 4"));
options.Add(new KeyValuePair<int, string>(5, "APM 2 analog pin 5"));
options.Add(new KeyValuePair<int, string>(6, "APM 2 analog pin 6"));
options.Add(new KeyValuePair<int, string>(7, "APM 2 analog pin 7"));
options.Add(new KeyValuePair<int, string>(8, "APM 2 analog pin 8"));
options.Add(new KeyValuePair<int, string>(9, "APM 2 analog pin 9"));

options.Add(new KeyValuePair<int, string>(64, "APM 1 AS Port"));

options.Add(new KeyValuePair<int, string>(11, "PX4 Analog AS Port"));
options.Add(new KeyValuePair<int, string>(15, "Pixhawk Analog AS Port"));
options.Add(new KeyValuePair<int, string>(65, "PX4/Pixhawk EagleTree or MEAS I2C AS Sensor"));

mavlinkCheckBoxAirspeed_pin.setup(options, "ARSPD_PIN", MainV2.comPort.MAV.param);


startup = false;
}

private void CHK_enableairspeed_CheckedChanged(object sender, EventArgs e)
{
if (startup)
return;
try
{
if (MainV2.comPort.MAV.param["ARSPD_ENABLE"] == null)
{
CustomMessageBox.Show(Strings.ErrorFeatureNotEnabled, Strings.ERROR);
}
else
{
MainV2.comPort.setParam("ARSPD_ENABLE", ((CheckBox) sender).Checked ? 1 : 0);
}
}
catch
{
CustomMessageBox.Show(string.Format(Strings.ErrorSetValueFailed, "ARSPD_ENABLE"), Strings.ERROR);
}
}
}
}
4 changes: 2 additions & 2 deletions GCSViews/ConfigurationView/ConfigHWBT.cs
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,8 @@ namespace MissionPlanner.GCSViews.ConfigurationView
{
public partial class ConfigHWBT : UserControl, IActivate
{
private const float rad2deg = (float) (180/Math.PI);
private const float deg2rad = (float) (1.0/rad2deg);
private const double rad2deg = (float) (180/Math.PI);
private const double deg2rad = (float) (1.0/rad2deg);
private static readonly ILog log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);

private readonly Dictionary<int, int> baudmap = new Dictionary<int, int>
Expand Down
Loading

0 comments on commit 3b3619b

Please sign in to comment.