public void normalize()
{
// '''re-normalise a rotation matrix'''
Vector3 error = self.a * self.b;
Vector3 t0 = self.a - (self.b * (0.5 * error));
Vector3 t1 = self.b - (self.a * (0.5 * error));
Vector3 t2 = t0 % t1;
self.a = t0 * (1.0 / t0.length());
self.b = t1 * (1.0 / t1.length());
self.c = t2 * (1.0 / t2.length());
}
public void normalize()
{
// '''re-normalise a rotation matrix'''
Vector3 error = a * b;
Vector3 t0 = a - (b * (0.5 * error));
Vector3 t1 = b - (a * (0.5 * error));
Vector3 t2 = t0 % t1;
a = t0 * (1.0 / t0.length());
b = t1 * (1.0 / t1.length());
c = t2 * (1.0 / t2.length());
}
//# Calculate drag.
public Vector3 drag(Vector3 velocity, double deltat = 0)//, testing=None)
{
//'''return current wind force in Earth frame. The velocity parameter is
// a Vector3 of the current velocity of the aircraft in earth frame, m/s//'''
//# (m/s, degrees) : wind vector as a magnitude and angle.
double speed, direction;
self.current(deltat, out speed, out direction);
//# speed = self.speed
//# direction = self.direction
//# Get the wind vector.
Vector3 w = toVec(speed, Utils.radians(direction));
double obj_speed = velocity.length();
//# Compute the angle between the object vector and wind vector by taking
//# the dot product and dividing by the magnitudes.
double alpha = 0;
double d = w.length() * obj_speed;
if (d == 0)
{
alpha = 0;
}
else
{
int checkme; // the python code this was copied from has an error
alpha = Utils.acos(((w * velocity).length() / d));
}
//# Get the relative wind speed and angle from the object. Note that the
//# relative wind speed includes the velocity of the object; i.e., there
//# is a headwind equivalent to the object's speed even if there is no
//# absolute wind.
double rel_speed, beta;
apparent_wind(speed, obj_speed, alpha, out rel_speed, out beta);
//# Return the vector of the relative wind, relative to the coordinate
//# system.
Vector3 relWindVec = toVec(rel_speed, beta + Utils.atan2(velocity.y, velocity.x));
//# Combine them to get the acceleration vector.
return(new Vector3(acc(relWindVec.x, drag_force(self, relWindVec.x))
, acc(relWindVec.y, drag_force(self, relWindVec.y))
, 0));
}
//# Calculate drag.
public Vector3 drag(Vector3 velocity, double deltat = 0) //, testing=None)
{
//'''return current wind force in Earth frame. The velocity parameter is
// a Vector3 of the current velocity of the aircraft in earth frame, m/s//'''
//# (m/s, degrees) : wind vector as a magnitude and angle.
double speed, direction;
self.current(deltat, out speed, out direction);
//# speed = self.speed
//# direction = self.direction
//# Get the wind vector.
Vector3 w = toVec(speed, Utils.radians(direction));
double obj_speed = velocity.length();
//# Compute the angle between the object vector and wind vector by taking
//# the dot product and dividing by the magnitudes.
double alpha = 0;
double d = w.length()*obj_speed;
if (d == 0)
{
alpha = 0;
}
else
{
//int checkme; // the python code this was copied from has an error
//alpha = Utils.acos(((w * velocity).length() / d));
}
//# Get the relative wind speed and angle from the object. Note that the
//# relative wind speed includes the velocity of the object; i.e., there
//# is a headwind equivalent to the object's speed even if there is no
//# absolute wind.
double rel_speed, beta;
apparent_wind(speed, obj_speed, alpha, out rel_speed, out beta);
//# Return the vector of the relative wind, relative to the coordinate
//# system.
Vector3 relWindVec = toVec(rel_speed, beta + Utils.atan2(velocity.y, velocity.x));
//# Combine them to get the acceleration vector.
return new Vector3(acc(relWindVec.x, drag_force(self, relWindVec.x))
, acc(relWindVec.y, drag_force(self, relWindVec.y))
, 0);
}
static string process(MAVLinkInterface mavint)
{
DateTime Deadline = DateTime.Now.AddSeconds(60);
Vector3 last_mag = null;
double last_usec = 0;
double count = 0;
double[] total_error = new double[rotations.Length];
float AHRS_ORIENTATION = 0;
float COMPASS_ORIENT = 0;
float COMPASS_EXTERNAL = 0;
//# now gather all the data
while (DateTime.Now < Deadline || mavint.BaseStream.BytesToRead > 0)
{
MAVLink.MAVLinkMessage packetbytes = mavint.readPacket();
if (packetbytes.Length < 5)
continue;
object packet = packetbytes.data;
if (packet == null)
continue;
if (packet is MAVLink.mavlink_param_value_t)
{
MAVLink.mavlink_param_value_t m = (MAVLink.mavlink_param_value_t) packet;
if (str(m.param_id) == "AHRS_ORIENTATION")
AHRS_ORIENTATION = (int) (m.param_value);
if (str(m.param_id) == "COMPASS_ORIENT")
COMPASS_ORIENT = (int) (m.param_value);
if (str(m.param_id) == "COMPASS_EXTERNAL")
COMPASS_EXTERNAL = (int) (m.param_value);
}
if (packet is MAVLink.mavlink_raw_imu_t)
{
MAVLink.mavlink_raw_imu_t m = (MAVLink.mavlink_raw_imu_t) packet;
Vector3 mag = new Vector3(m.xmag, m.ymag, m.zmag);
mag = mag_fixup(mag, AHRS_ORIENTATION, COMPASS_ORIENT, COMPASS_EXTERNAL);
Vector3 gyr = new Vector3(m.xgyro, m.ygyro, m.zgyro)*0.001;
double usec = m.time_usec;
if (last_mag != null && gyr.length() > radians(5.0))
{
add_errors(mag, gyr, last_mag, (usec - last_usec)*1.0e-6, total_error, rotations);
count += 1;
}
last_mag = mag;
last_usec = usec;
}
}
int best_i = 0;
double best_err = total_error[0];
foreach (var i in range(len(rotations)))
{
Rotation r = rotations[i];
// if (!r.is_90_degrees())
// continue;
//if ( opts.verbose) {
// print("%s err=%.2f" % (r, total_error[i]/count));}
if (total_error[i] < best_err)
{
best_i = i;
best_err = total_error[i];
}
}
Rotation rans = rotations[best_i];
Console.WriteLine("Best rotation is {0} err={1} from {2} points", rans, best_err/count, count);
//print("Best rotation is %s err=%.2f from %u points" % (r, best_err/count, count));
return rans.name;
}
//public double angle (Vector3 self, Vector3 v) {
// '''return the angle between this vector and another vector'''
// return Math.Acos(self * v) / (self.length() * v.length());
//}
public Vector3 normalized()
{
return(self / self.length());
}
private void Temp_Paint(object sender, PaintEventArgs e)
{
var rawdata = _dS.GetRaw();
e.Graphics.Clear(temp.BackColor);
var midx = e.ClipRectangle.Width / 2.0f;
var midy = e.ClipRectangle.Height / 2.0f;
//e.Graphics.DrawArc(System.Drawing.Pens.Green, midx - 10, midy - 10, 20, 20, 0, 360);
temp.Text = "Radius(+/-): " + (screenradius / 100.0) + "m MAV size([/]): " + (mavsize / 100.0) + "m";
// 11m radius = 22 m coverage
var scale = ((screenradius+50) * 2) / Math.Min(this.temp.Height,this.temp.Width);
// 80cm quad / scale
var size = mavsize / scale;
switch(_parent.cs.firmware)
{
case MainV2.Firmwares.ArduCopter2:
var imw = size/2;
e.Graphics.DrawImage(global::MissionPlanner.Properties.Resources.quadicon, midx - imw, midy - imw, size, size);
break;
}
foreach (var temp in rawdata.ToList())
{
Vector3 location = new Vector3(0, Math.Min(temp.Distance / scale, (screenradius) / scale), 0);
var halflength = location.length() / 2.0f;
var doublelength = location.length() * 2.0f;
var length = location.length();
Pen redpen = new Pen(Color.Red, 3);
float move = 5;
var font = new Font(Control.DefaultFont.FontFamily, Control.DefaultFont.Size, FontStyle.Bold);
switch (temp.Orientation)
{
case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_NONE:
location.rotate(Common.Rotation.ROTATION_NONE);
e.Graphics.DrawString((temp.Distance/100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x-move*2, midy - (float)location.y + move);
e.Graphics.DrawArc(redpen, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, -22.5f - 90f, 45f);
break;
case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_45:
location.rotate(Common.Rotation.ROTATION_YAW_45);
e.Graphics.DrawString((temp.Distance/100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x- move*8, midy - (float)location.y+ move);
e.Graphics.DrawArc(redpen, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 22.5f - 90f, 45f);
break;
case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_90:
location.rotate(Common.Rotation.ROTATION_YAW_90);
e.Graphics.DrawString((temp.Distance/100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x- move*8, midy - (float)location.y);
e.Graphics.DrawArc(redpen, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 67.5f - 90f, 45f);
break;
case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_135:
location.rotate(Common.Rotation.ROTATION_YAW_135);
e.Graphics.DrawString((temp.Distance/100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x- move*8, midy - (float)location.y- move);
e.Graphics.DrawArc(redpen, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 112.5f - 90f, 45f);
break;
case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_180:
location.rotate(Common.Rotation.ROTATION_YAW_180);
e.Graphics.DrawString((temp.Distance/100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x-move*2, midy - (float)location.y-move*3);
e.Graphics.DrawArc(redpen, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 157.5f - 90f, 45f);
break;
case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_225:
location.rotate(Common.Rotation.ROTATION_YAW_225);
e.Graphics.DrawString((temp.Distance/100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x+ move, midy - (float)location.y- move);
e.Graphics.DrawArc(redpen, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 202.5f - 90f, 45f);
break;
case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_270:
location.rotate(Common.Rotation.ROTATION_YAW_270);
e.Graphics.DrawString((temp.Distance/100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x+move, midy - (float)location.y);
e.Graphics.DrawArc(redpen, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 247.5f - 90f, 45f);
break;
case MAV_SENSOR_ORIENTATION.MAV_SENSOR_ROTATION_YAW_315:
location.rotate(Common.Rotation.ROTATION_YAW_315);
e.Graphics.DrawString((temp.Distance/100).ToString("0.0m"), font, System.Drawing.Brushes.Green, midx - (float)location.x+move, midy - (float)location.y);
e.Graphics.DrawArc(redpen, (float)(midx - length), (float)(midy - length), (float)doublelength, (float)doublelength, 292.5f - 90f, 45f);
break;
}
}
}
