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; } } }