void DoCalibration(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
var prd = ((ProgressReporterDialogue)sender);
prd.UpdateProgressAndStatus(-1, "Starting Compass Mot");
int8_t comp_type; // throttle or current based compensation
Vector3f compass_base = new Vector3f(); // compass vector when throttle is zero
Vector3f motor_impact = new Vector3f(); // impact of motors on compass vector
Vector3f motor_impact_scaled; // impact of motors on compass vector scaled with throttle
Vector3f motor_compensation; // final compensation to be stored to eeprom
float throttle_pct; // throttle as a percentage 0.0 ~ 1.0
float throttle_pct_max = 0.0f; // maximum throttle reached (as a percentage 0~1.0)
float current_amps_max = 0.0f; // maximum current reached
float interference_pct; // interference as a percentage of total mag field (for reporting purposes only)
uint32_t last_run_time;
uint8_t print_counter = 49;
bool updated = false; // have we updated the compensation vector at least once
if ((float)MainV2.comPort.param["BATT_MONITOR"] == 4f) // volt and current
{
comp_type = (sbyte)comptype.Current;
prd.UpdateProgressAndStatus(-1, "Compass Mot using current");
}
else
{
comp_type = (sbyte)comptype.Throttle;
prd.UpdateProgressAndStatus(-1, "Compass Mot using throttle");
}
if ((float)MainV2.comPort.param["COMPASS_USE"] != 1)
{
e.ErrorMessage = "Compass is disabled";
return;
}
// request streams
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, 10); // rc out
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 50); // mag out
// reset compass mot
MainV2.comPort.setParam("COMPASS_MOTCT ", 0.0f);
MainV2.comPort.setParam("COMPASS_MOT_X", 0.000000f);
MainV2.comPort.setParam("COMPASS_MOT_Y", 0.000000f);
MainV2.comPort.setParam("COMPASS_MOT_Z", 0.000000f);
// store initial x,y,z compass values
compass_base.x = MainV2.comPort.MAV.cs.mx;
compass_base.y = MainV2.comPort.MAV.cs.my;
compass_base.z = MainV2.comPort.MAV.cs.mz;
// initialise motor compensation
motor_compensation = new Vector3f(0, 0, 0);
int magseen = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_RAW_IMU];
int rcseen = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW];
DateTime deadline = DateTime.Now.AddSeconds(10);
prd.UpdateProgressAndStatus(-1, "Waiting for Mag and RC data");
while (true)
{
if (magseen > (magseen + 100) && rcseen > (rcseen + 20))
{
break;
}
if (e.CancelRequested)
{
e.CancelAcknowledged = true;
return;
}
if (DateTime.Now > deadline)
{
e.ErrorMessage = "Not enough packets where received\n" + magseen + " mag " + rcseen + " rc";
return;
}
}
while (true)
{
if (prd.doWorkArgs.CancelRequested)
{
prd.doWorkArgs.CancelAcknowledged = true;
break;
}
// radio
// passthorugh - cant do.
// compass read
// battery read
// calculate scaling for throttle
int checkme;
throttle_pct = (float)MainV2.comPort.MAV.cs.ch3percent / 100.0f;
throttle_pct = constrain_float(throttle_pct, 0.0f, 1.0f);
// if throttle is zero, update base x,y,z values
if (throttle_pct == 0.0f)
{
compass_base.x = compass_base.x * 0.99f + (float)MainV2.comPort.MAV.cs.mx * 0.01f;
compass_base.y = compass_base.y * 0.99f + (float)MainV2.comPort.MAV.cs.my * 0.01f;
compass_base.z = compass_base.z * 0.99f + (float)MainV2.comPort.MAV.cs.mz * 0.01f;
// causing printing to happen as soon as throttle is lifted
print_counter = 49;
}
else
{
// calculate diff from compass base and scale with throttle
motor_impact.x = MainV2.comPort.MAV.cs.mx - compass_base.x;
motor_impact.y = MainV2.comPort.MAV.cs.my - compass_base.y;
motor_impact.z = MainV2.comPort.MAV.cs.mz - compass_base.z;
// throttle based compensation
if (comp_type == (byte)comptype.Throttle)
{
// scale by throttle
motor_impact_scaled = motor_impact / throttle_pct;
// adjust the motor compensation to negate the impact
motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
updated = true;
}
else
{
// current based compensation if more than 3amps being drawn
motor_impact_scaled = motor_impact / MainV2.comPort.MAV.cs.current;
// adjust the motor compensation to negate the impact if drawing over 3amps
if (MainV2.comPort.MAV.cs.current >= 3.0f)
{
motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
updated = true;
}
}
// record maximum throttle and current
throttle_pct_max = max(throttle_pct_max, throttle_pct);
current_amps_max = max(current_amps_max, MainV2.comPort.MAV.cs.current);
// display output at 1hz if throttle is above zero
print_counter++;
if (print_counter >= 50)
{
print_counter = 0;
var line = String.Format("thr:%d cur:%4.2f mot x:%4.1f y:%4.1f z:%4.1f comp x:%4.2f y:%4.2f z:%4.2f\n", (int)MainV2.comPort.MAV.cs.ch3percent, (float)MainV2.comPort.MAV.cs.current, (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z);
Console.Write(line);
prd.UpdateProgressAndStatus(-1, line);
}
}
}
MainV2.comPort.doARM(false);
// set and save motor compensation
if (updated)
{
MainV2.comPort.setParam("COMPASS_MOTCT ", comp_type);
MainV2.comPort.setParam("COMPASS_MOT_X", (float)motor_compensation.x);
MainV2.comPort.setParam("COMPASS_MOT_Y", (float)motor_compensation.y);
MainV2.comPort.setParam("COMPASS_MOT_Z", (float)motor_compensation.z);
// calculate and display interference compensation at full throttle as % of total mag field
if (comp_type == (byte)comptype.Throttle)
{
// interference is impact@fullthrottle / mag field * 100
interference_pct = (float)motor_compensation.length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
}
else
{
// interference is impact/amp * (max current seen / max throttle seen) / mag field * 100
interference_pct = (float)motor_compensation.length() * (current_amps_max / throttle_pct_max) / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
}
string line = String.Format("\nInterference at full throttle is {0}% of mag field\n\n", (int)interference_pct);
Console.Write(line);
prd.UpdateProgressAndStatus(100, line);
}
else
{
prd.UpdateProgressAndStatus(100, "Failed");
}
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.ALL, 2);
}
void DoCalibration(object sender, ProgressWorkerEventArgs e, object passdata = null)
{
var prd = ((ProgressReporterDialogue)sender);
prd.UpdateProgressAndStatus(-1, "Starting Compass Mot");
int8_t comp_type; // throttle or current based compensation
Vector3f compass_base = new Vector3f(); // compass vector when throttle is zero
Vector3f motor_impact = new Vector3f(); // impact of motors on compass vector
Vector3f motor_impact_scaled; // impact of motors on compass vector scaled with throttle
Vector3f motor_compensation; // final compensation to be stored to eeprom
float throttle_pct; // throttle as a percentage 0.0 ~ 1.0
float throttle_pct_max = 0.0f; // maximum throttle reached (as a percentage 0~1.0)
float current_amps_max = 0.0f; // maximum current reached
float interference_pct; // interference as a percentage of total mag field (for reporting purposes only)
uint32_t last_run_time;
uint8_t print_counter = 49;
bool updated = false; // have we updated the compensation vector at least once
if ((float)MainV2.comPort.param["BATT_MONITOR"] == 4f) // volt and current
{
comp_type = (sbyte)comptype.Current;
prd.UpdateProgressAndStatus(-1, "Compass Mot using current");
}
else
{
comp_type = (sbyte)comptype.Throttle;
prd.UpdateProgressAndStatus(-1, "Compass Mot using throttle");
}
if ((float)MainV2.comPort.param["COMPASS_USE"] != 1)
{
e.ErrorMessage = "Compass is disabled";
return;
}
// request streams
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, 10); // rc out
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 50); // mag out
// reset compass mot
MainV2.comPort.setParam("COMPASS_MOTCT ", 0.0f);
MainV2.comPort.setParam("COMPASS_MOT_X", 0.000000f);
MainV2.comPort.setParam("COMPASS_MOT_Y", 0.000000f);
MainV2.comPort.setParam("COMPASS_MOT_Z", 0.000000f);
// store initial x,y,z compass values
compass_base.x = MainV2.comPort.MAV.cs.mx;
compass_base.y = MainV2.comPort.MAV.cs.my;
compass_base.z = MainV2.comPort.MAV.cs.mz;
// initialise motor compensation
motor_compensation = new Vector3f(0, 0, 0);
int magseen = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_RAW_IMU];
int rcseen = MainV2.comPort.MAV.packetseencount[MAVLink.MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW];
DateTime deadline = DateTime.Now.AddSeconds(10);
prd.UpdateProgressAndStatus(-1, "Waiting for Mag and RC data");
while (true)
{
if (magseen > (magseen + 100) && rcseen > (rcseen + 20))
{
break;
}
if (e.CancelRequested)
{
e.CancelAcknowledged = true;
return;
}
if (DateTime.Now > deadline)
{
e.ErrorMessage = "Not enough packets where received\n" + magseen + " mag " + rcseen + " rc";
return;
}
}
while (true)
{
if (prd.doWorkArgs.CancelRequested)
{
prd.doWorkArgs.CancelAcknowledged = true;
break;
}
// radio
// passthorugh - cant do.
// compass read
// battery read
// calculate scaling for throttle
int checkme;
throttle_pct = (float)MainV2.comPort.MAV.cs.ch3percent / 100.0f;
throttle_pct = constrain_float(throttle_pct, 0.0f, 1.0f);
// if throttle is zero, update base x,y,z values
if (throttle_pct == 0.0f)
{
compass_base.x = compass_base.x * 0.99f + (float)MainV2.comPort.MAV.cs.mx * 0.01f;
compass_base.y = compass_base.y * 0.99f + (float)MainV2.comPort.MAV.cs.my * 0.01f;
compass_base.z = compass_base.z * 0.99f + (float)MainV2.comPort.MAV.cs.mz * 0.01f;
// causing printing to happen as soon as throttle is lifted
print_counter = 49;
}
else
{
// calculate diff from compass base and scale with throttle
motor_impact.x = MainV2.comPort.MAV.cs.mx - compass_base.x;
motor_impact.y = MainV2.comPort.MAV.cs.my - compass_base.y;
motor_impact.z = MainV2.comPort.MAV.cs.mz - compass_base.z;
// throttle based compensation
if (comp_type == (byte)comptype.Throttle)
{
// scale by throttle
motor_impact_scaled = motor_impact / throttle_pct;
// adjust the motor compensation to negate the impact
motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
updated = true;
}
else
{
// current based compensation if more than 3amps being drawn
motor_impact_scaled = motor_impact / MainV2.comPort.MAV.cs.current;
// adjust the motor compensation to negate the impact if drawing over 3amps
if (MainV2.comPort.MAV.cs.current >= 3.0f)
{
motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
updated = true;
}
}
// record maximum throttle and current
throttle_pct_max = max(throttle_pct_max, throttle_pct);
current_amps_max = max(current_amps_max, MainV2.comPort.MAV.cs.current);
// display output at 1hz if throttle is above zero
print_counter++;
if (print_counter >= 50)
{
print_counter = 0;
var line = String.Format("thr:%d cur:%4.2f mot x:%4.1f y:%4.1f z:%4.1f comp x:%4.2f y:%4.2f z:%4.2f\n", (int)MainV2.comPort.MAV.cs.ch3percent, (float)MainV2.comPort.MAV.cs.current, (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z);
Console.Write(line);
prd.UpdateProgressAndStatus(-1, line);
}
}
}
MainV2.comPort.doARM(false);
// set and save motor compensation
if (updated)
{
MainV2.comPort.setParam("COMPASS_MOTCT ", comp_type);
MainV2.comPort.setParam("COMPASS_MOT_X", (float)motor_compensation.x);
MainV2.comPort.setParam("COMPASS_MOT_Y", (float)motor_compensation.y);
MainV2.comPort.setParam("COMPASS_MOT_Z", (float)motor_compensation.z);
// calculate and display interference compensation at full throttle as % of total mag field
if (comp_type == (byte)comptype.Throttle)
{
// interference is impact@fullthrottle / mag field * 100
interference_pct = (float)motor_compensation.length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
}
else
{
// interference is impact/amp * (max current seen / max throttle seen) / mag field * 100
interference_pct = (float)motor_compensation.length() * (current_amps_max / throttle_pct_max) / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
}
string line = String.Format("\nInterference at full throttle is {0}% of mag field\n\n", (int)interference_pct);
Console.Write(line);
prd.UpdateProgressAndStatus(100, line);
}
else
{
prd.UpdateProgressAndStatus(100, "Failed");
}
MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.ALL, 2);
}
