Skip to content

Commit

Permalink
GCSVews: Configuration: Compass2: support new compass param names
Browse files Browse the repository at this point in the history
  • Loading branch information
IamPete1 committed Nov 17, 2021
1 parent 0f83225 commit 0fe8296
Showing 1 changed file with 58 additions and 17 deletions.
75 changes: 58 additions & 17 deletions GCSViews/ConfigurationView/ConfigHWCompass2.cs
Original file line number Diff line number Diff line change
Expand Up @@ -27,20 +27,20 @@ public class CompassDeviceInfo : DeviceInfo
private string _orient;
private bool _external;

public CompassDeviceInfo(int index, string ParamName, uint id) : base(index, ParamName, id)
public CompassDeviceInfo(int index, string ParamName, uint id, ConfigHWCompass2 compass2) : base(index, ParamName, id)
{
//set initial state
var id1 = MainV2.comPort.MAV.param["COMPASS_DEV_ID"];
var id2 = MainV2.comPort.MAV.param["COMPASS_DEV_ID2"];
var id3 = MainV2.comPort.MAV.param["COMPASS_DEV_ID3"];
var id1 = MainV2.comPort.MAV.param[compass2.get_compass_param_name("DEV_ID", 1)];
var id2 = MainV2.comPort.MAV.param[compass2.get_compass_param_name("DEV_ID", 2)];
var id3 = MainV2.comPort.MAV.param[compass2.get_compass_param_name("DEV_ID", 3)];

var idO1 = MainV2.comPort.MAV.param["COMPASS_ORIENT"];
var idO2 = MainV2.comPort.MAV.param["COMPASS_ORIENT2"];
var idO3 = MainV2.comPort.MAV.param["COMPASS_ORIENT3"];
var idO1 = MainV2.comPort.MAV.param[compass2.get_compass_param_name("ORIENT", 1)];
var idO2 = MainV2.comPort.MAV.param[compass2.get_compass_param_name("ORIENT", 2)];
var idO3 = MainV2.comPort.MAV.param[compass2.get_compass_param_name("ORIENT", 3)];

var idE1 = MainV2.comPort.MAV.param["COMPASS_EXTERNAL"];
var idE2 = MainV2.comPort.MAV.param["COMPASS_EXTERN2"];
var idE3 = MainV2.comPort.MAV.param["COMPASS_EXTERN3"];
var idE1 = MainV2.comPort.MAV.param[compass2.get_compass_param_name("EXTERN", 1)];
var idE2 = MainV2.comPort.MAV.param[compass2.get_compass_param_name("EXTERN", 2)];
var idE3 = MainV2.comPort.MAV.param[compass2.get_compass_param_name("EXTERN", 3)];

if (id1 != null && id1?.Value == id)
{
Expand Down Expand Up @@ -85,14 +85,17 @@ public ConfigHWCompass2()

public void Activate()
{
// reset param version check
param_format_initialized = false;

// COMPASS_DEV_ID get a list of all connected devices
list = MainV2.comPort.MAV.param.Where(a => a.Name.StartsWith("COMPASS_DEV_ID") && a.Value != 0)
.Select((a, b) => new CompassDeviceInfo(b, a.Name, (uint) a.Value))
list = MainV2.comPort.MAV.param.Where(a => a.Name.StartsWith("COMPASS") && a.Name.Contains("DEV_ID") && a.Value != 0)
.Select((a, b) => new CompassDeviceInfo(b, a.Name, (uint) a.Value, this))
.OrderBy((a) => a.ParamName).ToList();

// COMPASS_PRIO get a list of all prios
var prio = MainV2.comPort.MAV.param.Where(a => a.Name.StartsWith("COMPASS_PRIO") && a.Value != 0)
.Select((a, b) => new CompassDeviceInfo(b, a.Name, (uint)a.Value))
.Select((a, b) => new CompassDeviceInfo(b, a.Name, (uint)a.Value, this))
.OrderBy((a) => a.ParamName).ToList();

var anymissing = false;
Expand Down Expand Up @@ -121,15 +124,15 @@ public void Activate()
mavlinkComboBoxfitness.setup(ParameterMetaDataRepository.GetParameterOptionsInt("COMPASS_CAL_FIT",
MainV2.comPort.MAV.cs.firmware.ToString()), "COMPASS_CAL_FIT", MainV2.comPort.MAV.param);

mavlinkCheckBoxUseCompass1.setup(1, 0, "COMPASS_USE", MainV2.comPort.MAV.param);
mavlinkCheckBoxUseCompass2.setup(1, 0, "COMPASS_USE2", MainV2.comPort.MAV.param);
mavlinkCheckBoxUseCompass3.setup(1, 0, "COMPASS_USE3", MainV2.comPort.MAV.param);
mavlinkCheckBoxUseCompass1.setup(1, 0, get_compass_param_name("USE", 1), MainV2.comPort.MAV.param);
mavlinkCheckBoxUseCompass2.setup(1, 0, get_compass_param_name("USE", 2), MainV2.comPort.MAV.param);
mavlinkCheckBoxUseCompass3.setup(1, 0, get_compass_param_name("USE", 3), MainV2.comPort.MAV.param);

CHK_compass_learn.setup(1, 0, "COMPASS_LEARN", MainV2.comPort.MAV.param);

{
// set the default items
var source = ParameterMetaDataRepository.GetParameterOptionsInt("COMPASS_ORIENT",
var source = ParameterMetaDataRepository.GetParameterOptionsInt(get_compass_param_name("ORIENT", 1),
MainV2.comPort.MAV.cs.firmware.ToString())
.Select(a => new KeyValuePair<string, string>(a.Key.ToString(), a.Value)).ToList();
Orientation.DataSource = source;
Expand Down Expand Up @@ -529,5 +532,43 @@ private async void but_missing_ClickAsync(object sender, EventArgs e)

await UpdateFirst3();
}

private bool param_format_initialized;
private bool param_format_new;
private string get_compass_param_name(string param, int index)
{
if (!param_format_initialized)
{
// first compass offset param as flag for new or old param scheme
param_format_new = false;
if (MainV2.comPort.MAV.param.ContainsKey("COMPASS1_OFS_X"))
{
param_format_new = true;
}
param_format_initialized = true;
}
if (!param_format_new && index == 1 && String.Equals(param, "EXTERN"))
{
// compass 1 extern is a special case with full external
return "COMPASS_EXTERNAL";
}

if (!param_format_new)
{
// old param format
if (index == 1)
{
// first compass does not give number
return "COMPASS_" + param;

}
return "COMPASS_" + param + index;
}
else
{
// new format always gives number
return "COMPASS" + index + "_" + param;
}
}
}
}

0 comments on commit 0fe8296

Please sign in to comment.