/* public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow) { UpdateCurrentSettings(bs, false, MainV2.comPort); } */ public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLink mavinterface) { if (DateTime.Now > lastupdate.AddMilliseconds(19) || updatenow) // 50 hz { lastupdate = DateTime.Now; if (DateTime.Now.Second != lastwindcalc.Second) { lastwindcalc = DateTime.Now; dowindcalc(); } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] != null) // status text { string logdata = DateTime.Now + " " + Encoding.ASCII.GetString(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT], 6, mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].Length - 6); int ind = logdata.IndexOf('\0'); if (ind != -1) logdata = logdata.Substring(0, ind); if (messages.Count > 5) { messages.RemoveAt(0); } messages.Add(logdata + "\n"); mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] = null; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] != null) // hil { var hil = new ArdupilotMega.MAVLink.__mavlink_rc_channels_scaled_t(); object temp = hil; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED], ref temp, 6); hil = (MAVLink.__mavlink_rc_channels_scaled_t)(temp); hilch1 = hil.chan1_scaled; hilch2 = hil.chan2_scaled; hilch3 = hil.chan3_scaled; hilch4 = hil.chan4_scaled; hilch5 = hil.chan5_scaled; hilch6 = hil.chan6_scaled; hilch7 = hil.chan7_scaled; hilch8 = hil.chan8_scaled; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] = null; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] != null) { MAVLink.__mavlink_nav_controller_output_t nav = new MAVLink.__mavlink_nav_controller_output_t(); object temp = nav; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT], ref temp, 6); nav = (MAVLink.__mavlink_nav_controller_output_t)(temp); nav_roll = nav.nav_roll; nav_pitch = nav.nav_pitch; nav_bearing = nav.nav_bearing; target_bearing = nav.target_bearing; wp_dist = nav.wp_dist; alt_error = nav.alt_error; aspd_error = nav.aspd_error; xtrack_error = nav.xtrack_error; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] = null; } #if MAVLINK10 if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT] != null) { ArdupilotMega.MAVLink.__mavlink_heartbeat_t hb = new ArdupilotMega.MAVLink.__mavlink_heartbeat_t(); object temp = hb; MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT], ref temp, 6); hb = (ArdupilotMega.MAVLink.__mavlink_heartbeat_t)(temp); string oldmode = mode; mode = "Unknown"; if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) != 0) { if (hb.type == (byte)MAVLink.MAV_TYPE.MAV_TYPE_FIXED_WING) { switch (hb.custom_mode) { case (byte)(Common.apmmodes.MANUAL): mode = "Manual"; break; case (byte)(Common.apmmodes.GUIDED): mode = "Guided"; break; case (byte)(Common.apmmodes.STABILIZE): mode = "Stabilize"; break; case (byte)(Common.apmmodes.FLY_BY_WIRE_A): mode = "FBW A"; break; case (byte)(Common.apmmodes.FLY_BY_WIRE_B): mode = "FBW B"; break; case (byte)(Common.apmmodes.AUTO): mode = "Auto"; break; case (byte)(Common.apmmodes.RTL): mode = "RTL"; break; case (byte)(Common.apmmodes.LOITER): mode = "Loiter"; break; case (byte)(Common.apmmodes.CIRCLE): mode = "Circle"; break; default: mode = "Unknown"; break; } } else if (hb.type == (byte)MAVLink.MAV_TYPE.MAV_TYPE_QUADROTOR) { switch (hb.custom_mode) { case (byte)(Common.ac2modes.STABILIZE): mode = "Stabilize"; break; case (byte)(Common.ac2modes.ACRO): mode = "Acro"; break; case (byte)(Common.ac2modes.ALT_HOLD): mode = "Alt Hold"; break; case (byte)(Common.ac2modes.AUTO): mode = "Auto"; break; case (byte)(Common.ac2modes.GUIDED): mode = "Guided"; break; case (byte)(Common.ac2modes.LOITER): mode = "Loiter"; break; case (byte)(Common.ac2modes.RTL): mode = "RTL"; break; case (byte)(Common.ac2modes.CIRCLE): mode = "Circle"; break; default: mode = "Unknown"; break; } } } if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True") { MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode"))); } } if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null) { ArdupilotMega.MAVLink.__mavlink_sys_status_t sysstatus = new ArdupilotMega.MAVLink.__mavlink_sys_status_t(); object temp = sysstatus; MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS], ref temp, 6); sysstatus = (ArdupilotMega.MAVLink.__mavlink_sys_status_t)(temp); battery_voltage = sysstatus.voltage_battery; battery_remaining = sysstatus.battery_remaining; packetdropremote = sysstatus.drop_rate_comm; //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null; } #else if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null) { ArdupilotMega.MAVLink.__mavlink_sys_status_t sysstatus = new ArdupilotMega.MAVLink.__mavlink_sys_status_t(); object temp = sysstatus; MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS], ref temp, 6); sysstatus = (ArdupilotMega.MAVLink.__mavlink_sys_status_t)(temp); string oldmode = mode; switch (sysstatus.mode) { case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_UNINIT: switch (sysstatus.nav_mode) { case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_GROUNDED: mode = "Initialising"; break; } break; case (byte)(100 + Common.ac2modes.STABILIZE): mode = "Stabilize"; break; case (byte)(100 + Common.ac2modes.ACRO): mode = "Acro"; break; case (byte)(100 + Common.ac2modes.ALT_HOLD): mode = "Alt Hold"; break; case (byte)(100 + Common.ac2modes.AUTO): mode = "Auto"; break; case (byte)(100 + Common.ac2modes.GUIDED): mode = "Guided"; break; case (byte)(100 + Common.ac2modes.LOITER): mode = "Loiter"; break; case (byte)(100 + Common.ac2modes.RTL): mode = "RTL"; break; case (byte)(100 + Common.ac2modes.CIRCLE): mode = "Circle"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL: mode = "Manual"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_GUIDED: mode = "Guided"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST1: mode = "Stabilize"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST2: mode = "FBW A"; // fall though old switch (sysstatus.nav_mode) { case (byte)1: mode = "FBW A"; break; case (byte)2: mode = "FBW B"; break; case (byte)3: mode = "FBW C"; break; } break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST3: mode = "FBW B"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO: switch (sysstatus.nav_mode) { case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_WAYPOINT: mode = "Auto"; break; case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_RETURNING: mode = "RTL"; break; case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_HOLD: case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LOITER: mode = "Loiter"; break; case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LIFTOFF: mode = "Takeoff"; break; case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LANDING: mode = "Land"; break; } break; default: mode = "Unknown"; break; } battery_voltage = sysstatus.vbat; battery_remaining = sysstatus.battery_remaining; packetdropremote = sysstatus.packet_drop; if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True") { MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode"))); } //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null; } #endif if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] != null) { var att = new ArdupilotMega.MAVLink.__mavlink_attitude_t(); object temp = att; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE], ref temp, 6); att = (MAVLink.__mavlink_attitude_t)(temp); roll = att.roll * rad2deg; pitch = att.pitch * rad2deg; yaw = att.yaw * rad2deg; // Console.WriteLine(roll + " " + pitch + " " + yaw); //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] = null; } #if MAVLINK10 if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT] != null) { var gps = new MAVLink.__mavlink_gps_raw_int_t(); object temp = gps; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT], ref temp, 6); gps = (MAVLink.__mavlink_gps_raw_int_t)(temp); lat = gps.lat * 1.0e-7f; lng = gps.lon * 1.0e-7f; // alt = gps.alt; // using vfr as includes baro calc gpsstatus = gps.fix_type; // Console.WriteLine("gpsfix {0}",gpsstatus); gpshdop = gps.eph; groundspeed = gps.vel * 1.0e-2f; groundcourse = gps.cog * 1.0e-2f; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null; } #else if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] != null) { var gps = new MAVLink.__mavlink_gps_raw_t(); object temp = gps; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW], ref temp, 6); gps = (MAVLink.__mavlink_gps_raw_t)(temp); lat = gps.lat; lng = gps.lon; // alt = gps.alt; // using vfr as includes baro calc gpsstatus = gps.fix_type; // Console.WriteLine("gpsfix {0}",gpsstatus); gpshdop = gps.eph; groundspeed = gps.v; groundcourse = gps.hdg; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null; } #endif if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS] != null) { var gps = new MAVLink.__mavlink_gps_status_t(); object temp = gps; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS], ref temp, 6); gps = (MAVLink.__mavlink_gps_status_t)(temp); satcount = gps.satellites_visible; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] != null) { var loc = new MAVLink.__mavlink_global_position_int_t(); object temp = loc; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT], ref temp, 6); loc = (MAVLink.__mavlink_global_position_int_t)(temp); //alt = loc.alt / 1000.0f; lat = loc.lat / 10000000.0f; lng = loc.lon / 10000000.0f; } #if MAVLINK10 if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT] != null) { ArdupilotMega.MAVLink.__mavlink_mission_current_t wpcur = new ArdupilotMega.MAVLink.__mavlink_mission_current_t(); object temp = wpcur; MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT], ref temp, 6); wpcur = (ArdupilotMega.MAVLink.__mavlink_mission_current_t)(temp); int oldwp = (int)wpno; wpno = wpcur.seq; if (oldwp != wpno && MainV2.speechenable && MainV2.getConfig("speechwaypointenabled") == "True") { MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint"))); } //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null; } #else if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION] != null) { var loc = new MAVLink.__mavlink_global_position_t(); object temp = loc; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION], ref temp, 6); loc = (MAVLink.__mavlink_global_position_t)(temp); alt = loc.alt; lat = loc.lat; lng = loc.lon; } if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] != null) { ArdupilotMega.MAVLink.__mavlink_waypoint_current_t wpcur = new ArdupilotMega.MAVLink.__mavlink_waypoint_current_t(); object temp = wpcur; MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT], ref temp, 6); wpcur = (ArdupilotMega.MAVLink.__mavlink_waypoint_current_t)(temp); int oldwp = (int)wpno; wpno = wpcur.seq; if (oldwp != wpno && MainV2.speechenable && MainV2.getConfig("speechwaypointenabled") == "True") { MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint"))); } //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null; } #endif if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] != null) { var rcin = new MAVLink.__mavlink_rc_channels_raw_t(); object temp = rcin; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW], ref temp, 6); rcin = (MAVLink.__mavlink_rc_channels_raw_t)(temp); ch1in = rcin.chan1_raw; ch2in = rcin.chan2_raw; ch3in = rcin.chan3_raw; ch4in = rcin.chan4_raw; ch5in = rcin.chan5_raw; ch6in = rcin.chan6_raw; ch7in = rcin.chan7_raw; ch8in = rcin.chan8_raw; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] = null; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] != null) { var servoout = new MAVLink.__mavlink_servo_output_raw_t(); object temp = servoout; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW], ref temp, 6); servoout = (MAVLink.__mavlink_servo_output_raw_t)(temp); ch1out = servoout.servo1_raw; ch2out = servoout.servo2_raw; ch3out = servoout.servo3_raw; ch4out = servoout.servo4_raw; ch5out = servoout.servo5_raw; ch6out = servoout.servo6_raw; ch7out = servoout.servo7_raw; ch8out = servoout.servo8_raw; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] = null; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] != null) { var imu = new MAVLink.__mavlink_raw_imu_t(); object temp = imu; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU], ref temp, 6); imu = (MAVLink.__mavlink_raw_imu_t)(temp); gx = imu.xgyro; gy = imu.ygyro; gz = imu.zgyro; ax = imu.xacc; ay = imu.yacc; az = imu.zacc; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU] != null) { var imu = new MAVLink.__mavlink_scaled_imu_t(); object temp = imu; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU], ref temp, 6); imu = (MAVLink.__mavlink_scaled_imu_t)(temp); gx = imu.xgyro; gy = imu.ygyro; gz = imu.zgyro; ax = imu.xacc; ay = imu.yacc; az = imu.zacc; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] != null) { MAVLink.__mavlink_vfr_hud_t vfr = new MAVLink.__mavlink_vfr_hud_t(); object temp = vfr; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD], ref temp, 6); vfr = (MAVLink.__mavlink_vfr_hud_t)(temp); groundspeed = vfr.groundspeed; airspeed = vfr.airspeed; alt = vfr.alt; // this might include baro //climbrate = vfr.climb; if ((DateTime.Now - lastalt).TotalSeconds >= 0.1 && oldalt != alt) { climbrate = (alt - oldalt) / (float)(DateTime.Now - lastalt).TotalSeconds; verticalspeed = (alt - oldalt) / (float)(DateTime.Now - lastalt).TotalSeconds; if (float.IsInfinity(_verticalspeed)) _verticalspeed = 0; lastalt = DateTime.Now; oldalt = alt; } //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] = null; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO] != null) // hil { var mem = new ArdupilotMega.MAVLink.__mavlink_meminfo_t(); object temp = mem; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO], ref temp, 6); mem = (MAVLink.__mavlink_meminfo_t)(temp); freemem = mem.freemem; brklevel = mem.brkval; } } //Console.WriteLine(DateTime.Now.Millisecond + " start "); // update form try { if (bs != null) { //Console.WriteLine(DateTime.Now.Millisecond); bs.DataSource = this; //Console.WriteLine(DateTime.Now.Millisecond + " 1 " + updatenow); bs.ResetBindings(false); //Console.WriteLine(DateTime.Now.Millisecond + " done "); } } catch { Console.WriteLine("CurrentState Binding error"); } }
public void update(ref double[] servos, ArdupilotMega.GCSViews.Simulation.FGNetFDM fdm) { for (int i = 0; i < servos.Length; i++) { if (servos[i] <= 0.0) { motor_speed[i] = 0; } else { motor_speed[i] = scale_rc(i, (float)servos[i], 0.0f, 1.0f); //servos[i] = motor_speed[i]; } } double[] m = motor_speed; /* * roll = 0; * pitch = 0; * yaw = 0; * roll_rate = 0; * pitch_rate = 0; * yaw_rate = 0; */ // Console.WriteLine("\nin m {0:0.000000} {1:0.000000} {2:0.000000} {3:0.000000}", m[0], m[1], m[2], m[3]); // Console.WriteLine("in vel {0:0.000000} {1:0.000000} {2:0.000000}", velocity.X, velocity.Y, velocity.Z); //Console.WriteLine("in r {0:0.000000} p {1:0.000000} y {2:0.000000} - r {3:0.000000} p {4:0.000000} y {5:0.000000}", roll, pitch, yaw, roll_rate, pitch_rate, yaw_rate); // m[0] *= 0.5; //# how much time has passed? DateTime t = DateTime.Now; TimeSpan delta_time = t - last_time; // 0.02 last_time = t; if (delta_time.TotalMilliseconds > 100) // somethings wrong / debug { delta_time = new TimeSpan(0, 0, 0, 0, 20); } // rotational acceleration, in degrees/s/s, in body frame double roll_accel = 0.0; double pitch_accel = 0.0; double yaw_accel = 0.0; double thrust = 0.0; foreach (var i in range((self.motors.Length))) { roll_accel += -5000.0 * sin(radians(self.motors[i].angle)) * m[i]; pitch_accel += 5000.0 * cos(radians(self.motors[i].angle)) * m[i]; if (self.motors[i].clockwise) { yaw_accel -= m[i] * 400.0; } else { yaw_accel += m[i] * 400.0; } thrust += m[i] * self.thrust_scale; // newtons } // rotational resistance roll_accel -= (self.pDeg / self.terminal_rotation_rate) * 5000.0; pitch_accel -= (self.qDeg / self.terminal_rotation_rate) * 5000.0; yaw_accel -= (self.rDeg / self.terminal_rotation_rate) * 400.0; //Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll); //# update rotational rates in body frame self.pDeg += roll_accel * delta_time.TotalSeconds; self.qDeg += pitch_accel * delta_time.TotalSeconds; self.rDeg += yaw_accel * delta_time.TotalSeconds; // Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll); // calculate rates in earth frame var answer = BodyRatesToEarthRates(self.roll, self.pitch, self.yaw, self.pDeg, self.qDeg, self.rDeg); self.roll_rate = answer.Item1; self.pitch_rate = answer.Item2; self.yaw_rate = answer.Item3; //self.roll_rate = pDeg; //self.pitch_rate = qDeg; //self.yaw_rate = rDeg; //# update rotation roll += roll_rate * delta_time.TotalSeconds; pitch += pitch_rate * delta_time.TotalSeconds; yaw += yaw_rate * delta_time.TotalSeconds; // Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll); //Console.WriteLine("r {0:0.0} p {1:0.0} y {2:0.0} - r {3:0.0} p {4:0.0} y {5:0.0} ms {6:0.000}", roll, pitch, yaw, roll_rate, pitch_rate, yaw_rate, delta_time.TotalSeconds); //# air resistance Vector3d air_resistance = -velocity * (gravity / terminal_velocity); //# normalise rotations normalise(); double accel = thrust / mass; //Console.WriteLine("in {0:0.000000} {1:0.000000} {2:0.000000} {3:0.000000}", roll, pitch, yaw, accel); Vector3d accel3D = RPY_to_XYZ(roll, pitch, yaw, accel); //Console.WriteLine("accel3D " + accel3D.X + " " + accel3D.Y + " " + accel3D.Z); accel3D += new Vector3d(0, 0, -gravity); accel3D += air_resistance; Random rand = new Random(); //velocity.X += .02 + rand.NextDouble() * .03; //velocity.Y += .02 + rand.NextDouble() * .03; //# new velocity vector velocity += accel3D * delta_time.TotalSeconds; this.accel = accel3D; //# new position vector old_position = new Vector3d(position); position += velocity * delta_time.TotalSeconds; // Console.WriteLine(fdm.agl + " "+ fdm.altitude); //Console.WriteLine("Z {0} halt {1} < gl {2} fh {3}" ,position.Z , home_altitude , ground_level , frame_height); if (home_latitude == 0 || home_latitude > 90 || home_latitude < -90 || home_longitude == 0) { this.home_latitude = fdm.latitude * rad2deg; this.home_longitude = fdm.longitude * rad2deg; this.home_altitude = fdm.altitude * ft2m; this.ground_level = this.home_altitude; this.altitude = fdm.altitude * ft2m; this.latitude = fdm.latitude * rad2deg; this.longitude = fdm.longitude * rad2deg; } //# constrain height to the ground if (position.Z + home_altitude < ground_level + frame_height) { if (old_position.Z + home_altitude > ground_level + frame_height) { // Console.WriteLine("Hit ground at {0} m/s", (-velocity.Z)); } velocity = new Vector3d(0, 0, 0); roll_rate = 0; pitch_rate = 0; yaw_rate = 0; roll = 0; pitch = 0; position = new Vector3d(position.X, position.Y, ground_level + frame_height - home_altitude + 0); // Console.WriteLine("here " + position.Z); } //# update lat/lon/altitude update_position(); // send to apm #if MAVLINK10 ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t(); #else ArdupilotMega.MAVLink.__mavlink_gps_raw_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_t(); #endif ArdupilotMega.MAVLink.__mavlink_attitude_t att = new ArdupilotMega.MAVLink.__mavlink_attitude_t(); ArdupilotMega.MAVLink.__mavlink_vfr_hud_t asp = new ArdupilotMega.MAVLink.__mavlink_vfr_hud_t(); att.roll = (float)roll * deg2rad; att.pitch = (float)pitch * deg2rad; att.yaw = (float)yaw * deg2rad; att.rollspeed = (float)roll_rate * deg2rad; att.pitchspeed = (float)pitch_rate * deg2rad; att.yawspeed = (float)yaw_rate * deg2rad; #if MAVLINK10 gps.alt = ((int)(altitude * 1000)); gps.fix_type = 3; gps.vel = (ushort)(Math.Sqrt((velocity.X * velocity.X) + (velocity.Y * velocity.Y)) * 100); gps.cog = (ushort)((((Math.Atan2(velocity.Y, velocity.X) * rad2deg) + 360) % 360) * 100); gps.lat = (int)(latitude * 1.0e7); gps.lon = (int)(longitude * 1.0e7); gps.time_usec = ((ulong)DateTime.Now.Ticks); asp.airspeed = gps.vel; #else gps.alt = ((float)(altitude)); gps.fix_type = 3; gps.lat = ((float)latitude); gps.lon = ((float)longitude); gps.usec = ((ulong)DateTime.Now.Ticks); //Random rand = new Random(); //gps.alt += (rand.Next(100) - 50) / 100.0f; //gps.lat += (float)((rand.NextDouble() - 0.5) * 0.00005); //gps.lon += (float)((rand.NextDouble() - 0.5) * 0.00005); //gps.v += (float)(rand.NextDouble() - 0.5) * 1.0f; gps.v = ((float)Math.Sqrt((velocity.X * velocity.X) + (velocity.Y * velocity.Y))); gps.hdg = (float)(((Math.Atan2(velocity.Y, velocity.X) * rad2deg) + 360) % 360);; asp.airspeed = gps.v; #endif MainV2.comPort.sendPacket(att); MAVLink.__mavlink_raw_pressure_t pres = new MAVLink.__mavlink_raw_pressure_t(); double calc = (101325 * Math.Pow(1 - 2.25577 * Math.Pow(10, -5) * gps.alt, 5.25588)); pres.press_diff1 = (short)(int)(calc); // 0 alt is 0 pa MainV2.comPort.sendPacket(pres); MainV2.comPort.sendPacket(asp); if (framecount % 120 == 0) {// 50 / 10 = 5 hz MainV2.comPort.sendPacket(gps); //Console.WriteLine(DateTime.Now.Millisecond + " GPS" ); } framecount++; }
/* * public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow) * { * UpdateCurrentSettings(bs, false, MainV2.comPort); * } */ public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLink mavinterface) { if (DateTime.Now > lastupdate.AddMilliseconds(19) || updatenow) // 50 hz { lastupdate = DateTime.Now; if (DateTime.Now.Second != lastwindcalc.Second) { lastwindcalc = DateTime.Now; dowindcalc(); } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] != null) // status text { string logdata = DateTime.Now + " " + Encoding.ASCII.GetString(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT], 6, mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].Length - 6); int ind = logdata.IndexOf('\0'); if (ind != -1) { logdata = logdata.Substring(0, ind); } if (messages.Count > 5) { messages.RemoveAt(0); } messages.Add(logdata + "\n"); mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] = null; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] != null) // hil { var hil = new ArdupilotMega.MAVLink.__mavlink_rc_channels_scaled_t(); object temp = hil; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED], ref temp, 6); hil = (MAVLink.__mavlink_rc_channels_scaled_t)(temp); hilch1 = hil.chan1_scaled; hilch2 = hil.chan2_scaled; hilch3 = hil.chan3_scaled; hilch4 = hil.chan4_scaled; hilch5 = hil.chan5_scaled; hilch6 = hil.chan6_scaled; hilch7 = hil.chan7_scaled; hilch8 = hil.chan8_scaled; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] = null; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] != null) { MAVLink.__mavlink_nav_controller_output_t nav = new MAVLink.__mavlink_nav_controller_output_t(); object temp = nav; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT], ref temp, 6); nav = (MAVLink.__mavlink_nav_controller_output_t)(temp); nav_roll = nav.nav_roll; nav_pitch = nav.nav_pitch; nav_bearing = nav.nav_bearing; target_bearing = nav.target_bearing; wp_dist = nav.wp_dist; alt_error = nav.alt_error; aspd_error = nav.aspd_error; xtrack_error = nav.xtrack_error; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] = null; } #if MAVLINK10 if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT] != null) { ArdupilotMega.MAVLink.__mavlink_heartbeat_t hb = new ArdupilotMega.MAVLink.__mavlink_heartbeat_t(); object temp = hb; MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT], ref temp, 6); hb = (ArdupilotMega.MAVLink.__mavlink_heartbeat_t)(temp); string oldmode = mode; mode = "Unknown"; if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) != 0) { if (hb.type == (byte)MAVLink.MAV_TYPE.MAV_TYPE_FIXED_WING) { switch (hb.custom_mode) { case (byte)(Common.apmmodes.MANUAL): mode = "Manual"; break; case (byte)(Common.apmmodes.GUIDED): mode = "Guided"; break; case (byte)(Common.apmmodes.STABILIZE): mode = "Stabilize"; break; case (byte)(Common.apmmodes.FLY_BY_WIRE_A): mode = "FBW A"; break; case (byte)(Common.apmmodes.FLY_BY_WIRE_B): mode = "FBW B"; break; case (byte)(Common.apmmodes.AUTO): mode = "Auto"; break; case (byte)(Common.apmmodes.RTL): mode = "RTL"; break; case (byte)(Common.apmmodes.LOITER): mode = "Loiter"; break; case (byte)(Common.apmmodes.CIRCLE): mode = "Circle"; break; default: mode = "Unknown"; break; } } else if (hb.type == (byte)MAVLink.MAV_TYPE.MAV_TYPE_QUADROTOR) { switch (hb.custom_mode) { case (byte)(Common.ac2modes.STABILIZE): mode = "Stabilize"; break; case (byte)(Common.ac2modes.ACRO): mode = "Acro"; break; case (byte)(Common.ac2modes.ALT_HOLD): mode = "Alt Hold"; break; case (byte)(Common.ac2modes.AUTO): mode = "Auto"; break; case (byte)(Common.ac2modes.GUIDED): mode = "Guided"; break; case (byte)(Common.ac2modes.LOITER): mode = "Loiter"; break; case (byte)(Common.ac2modes.RTL): mode = "RTL"; break; case (byte)(Common.ac2modes.CIRCLE): mode = "Circle"; break; default: mode = "Unknown"; break; } } } if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True") { MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode"))); } } if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null) { ArdupilotMega.MAVLink.__mavlink_sys_status_t sysstatus = new ArdupilotMega.MAVLink.__mavlink_sys_status_t(); object temp = sysstatus; MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS], ref temp, 6); sysstatus = (ArdupilotMega.MAVLink.__mavlink_sys_status_t)(temp); battery_voltage = sysstatus.voltage_battery; battery_remaining = sysstatus.battery_remaining; packetdropremote = sysstatus.drop_rate_comm; //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null; } #else if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null) { ArdupilotMega.MAVLink.__mavlink_sys_status_t sysstatus = new ArdupilotMega.MAVLink.__mavlink_sys_status_t(); object temp = sysstatus; MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS], ref temp, 6); sysstatus = (ArdupilotMega.MAVLink.__mavlink_sys_status_t)(temp); string oldmode = mode; switch (sysstatus.mode) { case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_UNINIT: switch (sysstatus.nav_mode) { case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_GROUNDED: mode = "Initialising"; break; } break; case (byte)(100 + Common.ac2modes.STABILIZE): mode = "Stabilize"; break; case (byte)(100 + Common.ac2modes.ACRO): mode = "Acro"; break; case (byte)(100 + Common.ac2modes.ALT_HOLD): mode = "Alt Hold"; break; case (byte)(100 + Common.ac2modes.AUTO): mode = "Auto"; break; case (byte)(100 + Common.ac2modes.GUIDED): mode = "Guided"; break; case (byte)(100 + Common.ac2modes.LOITER): mode = "Loiter"; break; case (byte)(100 + Common.ac2modes.RTL): mode = "RTL"; break; case (byte)(100 + Common.ac2modes.CIRCLE): mode = "Circle"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL: mode = "Manual"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_GUIDED: mode = "Guided"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST1: mode = "Stabilize"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST2: mode = "FBW A"; // fall though old switch (sysstatus.nav_mode) { case (byte)1: mode = "FBW A"; break; case (byte)2: mode = "FBW B"; break; case (byte)3: mode = "FBW C"; break; } break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST3: mode = "Circle"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO: switch (sysstatus.nav_mode) { case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_WAYPOINT: mode = "Auto"; break; case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_RETURNING: mode = "RTL"; break; case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_HOLD: case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LOITER: mode = "Loiter"; break; case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LIFTOFF: mode = "Takeoff"; break; case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LANDING: mode = "Land"; break; } break; default: mode = "Unknown"; break; } battery_voltage = sysstatus.vbat; battery_remaining = sysstatus.battery_remaining; packetdropremote = sysstatus.packet_drop; if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True") { MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode"))); } //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null; } #endif if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] != null) { var att = new ArdupilotMega.MAVLink.__mavlink_attitude_t(); object temp = att; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE], ref temp, 6); att = (MAVLink.__mavlink_attitude_t)(temp); roll = att.roll * rad2deg; pitch = att.pitch * rad2deg; yaw = att.yaw * rad2deg; // Console.WriteLine(roll + " " + pitch + " " + yaw); //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] = null; } #if MAVLINK10 if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT] != null) { var gps = new MAVLink.__mavlink_gps_raw_int_t(); object temp = gps; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT], ref temp, 6); gps = (MAVLink.__mavlink_gps_raw_int_t)(temp); lat = gps.lat * 1.0e-7f; lng = gps.lon * 1.0e-7f; // alt = gps.alt; // using vfr as includes baro calc gpsstatus = gps.fix_type; // Console.WriteLine("gpsfix {0}",gpsstatus); gpshdop = gps.eph; groundspeed = gps.vel * 1.0e-2f; groundcourse = gps.cog * 1.0e-2f; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null; } #else if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] != null) { var gps = new MAVLink.__mavlink_gps_raw_t(); object temp = gps; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW], ref temp, 6); gps = (MAVLink.__mavlink_gps_raw_t)(temp); lat = gps.lat; lng = gps.lon; // alt = gps.alt; // using vfr as includes baro calc gpsstatus = gps.fix_type; // Console.WriteLine("gpsfix {0}",gpsstatus); gpshdop = gps.eph; groundspeed = gps.v; groundcourse = gps.hdg; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null; } #endif if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS] != null) { var gps = new MAVLink.__mavlink_gps_status_t(); object temp = gps; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS], ref temp, 6); gps = (MAVLink.__mavlink_gps_status_t)(temp); satcount = gps.satellites_visible; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] != null) { var loc = new MAVLink.__mavlink_global_position_int_t(); object temp = loc; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT], ref temp, 6); loc = (MAVLink.__mavlink_global_position_int_t)(temp); //alt = loc.alt / 1000.0f; lat = loc.lat / 10000000.0f; lng = loc.lon / 10000000.0f; } #if MAVLINK10 if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT] != null) { ArdupilotMega.MAVLink.__mavlink_mission_current_t wpcur = new ArdupilotMega.MAVLink.__mavlink_mission_current_t(); object temp = wpcur; MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT], ref temp, 6); wpcur = (ArdupilotMega.MAVLink.__mavlink_mission_current_t)(temp); int oldwp = (int)wpno; wpno = wpcur.seq; if (oldwp != wpno && MainV2.speechenable && MainV2.getConfig("speechwaypointenabled") == "True") { MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint"))); } //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null; } #else if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION] != null) { var loc = new MAVLink.__mavlink_global_position_t(); object temp = loc; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION], ref temp, 6); loc = (MAVLink.__mavlink_global_position_t)(temp); alt = loc.alt; lat = loc.lat; lng = loc.lon; } if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] != null) { ArdupilotMega.MAVLink.__mavlink_waypoint_current_t wpcur = new ArdupilotMega.MAVLink.__mavlink_waypoint_current_t(); object temp = wpcur; MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT], ref temp, 6); wpcur = (ArdupilotMega.MAVLink.__mavlink_waypoint_current_t)(temp); int oldwp = (int)wpno; wpno = wpcur.seq; if (oldwp != wpno && MainV2.speechenable && MainV2.getConfig("speechwaypointenabled") == "True") { MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint"))); } //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null; } #endif if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] != null) { var rcin = new MAVLink.__mavlink_rc_channels_raw_t(); object temp = rcin; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW], ref temp, 6); rcin = (MAVLink.__mavlink_rc_channels_raw_t)(temp); ch1in = rcin.chan1_raw; ch2in = rcin.chan2_raw; ch3in = rcin.chan3_raw; ch4in = rcin.chan4_raw; ch5in = rcin.chan5_raw; ch6in = rcin.chan6_raw; ch7in = rcin.chan7_raw; ch8in = rcin.chan8_raw; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] = null; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] != null) { var servoout = new MAVLink.__mavlink_servo_output_raw_t(); object temp = servoout; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW], ref temp, 6); servoout = (MAVLink.__mavlink_servo_output_raw_t)(temp); ch1out = servoout.servo1_raw; ch2out = servoout.servo2_raw; ch3out = servoout.servo3_raw; ch4out = servoout.servo4_raw; ch5out = servoout.servo5_raw; ch6out = servoout.servo6_raw; ch7out = servoout.servo7_raw; ch8out = servoout.servo8_raw; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] = null; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] != null) { var imu = new MAVLink.__mavlink_raw_imu_t(); object temp = imu; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU], ref temp, 6); imu = (MAVLink.__mavlink_raw_imu_t)(temp); gx = imu.xgyro; gy = imu.ygyro; gz = imu.zgyro; ax = imu.xacc; ay = imu.yacc; az = imu.zacc; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU] != null) { var imu = new MAVLink.__mavlink_scaled_imu_t(); object temp = imu; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU], ref temp, 6); imu = (MAVLink.__mavlink_scaled_imu_t)(temp); gx = imu.xgyro; gy = imu.ygyro; gz = imu.zgyro; ax = imu.xacc; ay = imu.yacc; az = imu.zacc; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] != null) { MAVLink.__mavlink_vfr_hud_t vfr = new MAVLink.__mavlink_vfr_hud_t(); object temp = vfr; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD], ref temp, 6); vfr = (MAVLink.__mavlink_vfr_hud_t)(temp); groundspeed = vfr.groundspeed; airspeed = vfr.airspeed; alt = vfr.alt; // this might include baro //climbrate = vfr.climb; if ((DateTime.Now - lastalt).TotalSeconds >= 0.1 && oldalt != alt) { climbrate = (alt - oldalt) / (float)(DateTime.Now - lastalt).TotalSeconds; verticalspeed = (alt - oldalt) / (float)(DateTime.Now - lastalt).TotalSeconds; if (float.IsInfinity(_verticalspeed)) { _verticalspeed = 0; } lastalt = DateTime.Now; oldalt = alt; } //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] = null; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO] != null) // hil { var mem = new ArdupilotMega.MAVLink.__mavlink_meminfo_t(); object temp = mem; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO], ref temp, 6); mem = (MAVLink.__mavlink_meminfo_t)(temp); freemem = mem.freemem; brklevel = mem.brkval; } } //Console.WriteLine(DateTime.Now.Millisecond + " start "); // update form try { if (bs != null) { //Console.WriteLine(DateTime.Now.Millisecond); bs.DataSource = this; //Console.WriteLine(DateTime.Now.Millisecond + " 1 " + updatenow); bs.ResetBindings(false); //Console.WriteLine(DateTime.Now.Millisecond + " done "); } } catch { Console.WriteLine("CurrentState Binding error"); } }
/// <summary> /// Recevied UDP packet, process and send required data to serial port. /// </summary> /// <param name="data">Packet</param> /// <param name="receviedbytes">Length</param> /// <param name="comPort">Com Port</param> private void RECVprocess(byte[] data, int receviedbytes, ArdupilotMega.MAVLink comPort) { #if MAVLINK10 ArdupilotMega.MAVLink.__mavlink_hil_state_t hilstate = new ArdupilotMega.MAVLink.__mavlink_hil_state_t(); ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t(); #else ArdupilotMega.MAVLink.__mavlink_gps_raw_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_t(); #endif ArdupilotMega.MAVLink.__mavlink_raw_imu_t imu = new ArdupilotMega.MAVLink.__mavlink_raw_imu_t(); ArdupilotMega.MAVLink.__mavlink_attitude_t att = new ArdupilotMega.MAVLink.__mavlink_attitude_t(); ArdupilotMega.MAVLink.__mavlink_vfr_hud_t asp = new ArdupilotMega.MAVLink.__mavlink_vfr_hud_t(); if (data[0] == 'D' && data[1] == 'A') { // Xplanes sends // 5 byte header // 1 int for the index - numbers on left of output // 8 floats - might be useful. or 0 if not int count = 5; while (count < receviedbytes) { int index = BitConverter.ToInt32(data, count); DATA[index] = new float[8]; DATA[index][0] = BitConverter.ToSingle(data, count + 1 * 4); ; DATA[index][1] = BitConverter.ToSingle(data, count + 2 * 4); ; DATA[index][2] = BitConverter.ToSingle(data, count + 3 * 4); ; DATA[index][3] = BitConverter.ToSingle(data, count + 4 * 4); ; DATA[index][4] = BitConverter.ToSingle(data, count + 5 * 4); ; DATA[index][5] = BitConverter.ToSingle(data, count + 6 * 4); ; DATA[index][6] = BitConverter.ToSingle(data, count + 7 * 4); ; DATA[index][7] = BitConverter.ToSingle(data, count + 8 * 4); ; count += 36; // 8 * float } att.pitch = (DATA[18][0] * deg2rad); att.roll = (DATA[18][1] * deg2rad); att.yaw = (DATA[18][2] * deg2rad); att.pitchspeed = (DATA[17][0]); att.rollspeed = (DATA[17][1]); att.yawspeed = (DATA[17][2]); TimeSpan timediff = DateTime.Now - oldtime; float pdiff = (float)((att.pitch - oldatt.pitch) / timediff.TotalSeconds); float rdiff = (float)((att.roll - oldatt.roll) / timediff.TotalSeconds); float ydiff = (float)((att.yaw - oldatt.yaw) / timediff.TotalSeconds); //Console.WriteLine("{0:0.00000} {1:0.00000} {2:0.00000} \t {3:0.00000} {4:0.00000} {5:0.00000}", pdiff, rdiff, ydiff, DATA[17][0], DATA[17][1], DATA[17][2]); oldatt = att; rdiff = DATA[17][1]; pdiff = DATA[17][0]; ydiff = DATA[17][2]; Int16 xgyro = Constrain(rdiff * 1000.0, Int16.MinValue, Int16.MaxValue); Int16 ygyro = Constrain(pdiff * 1000.0, Int16.MinValue, Int16.MaxValue); Int16 zgyro = Constrain(ydiff * 1000.0, Int16.MinValue, Int16.MaxValue); oldtime = DateTime.Now; YLScsDrawing.Drawing3d.Vector3d accel3D = HIL.QuadCopter.RPY_to_XYZ(DATA[18][1], DATA[18][0], 0, -9.8); //DATA[18][2] float turnrad = (float)(((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704) * 1.943844) / (float)(11.26 * Math.Tan(att.roll))) * ft2m; float centripaccel = (float)((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / turnrad; //Console.WriteLine("old {0} {1} {2}",accel3D.X,accel3D.Y,accel3D.Z); YLScsDrawing.Drawing3d.Vector3d cent3D = HIL.QuadCopter.RPY_to_XYZ(DATA[18][1] - 90, 0, 0, centripaccel); accel3D -= cent3D; //Console.WriteLine("new {0} {1} {2}", accel3D.X, accel3D.Y, accel3D.Z); oldax = DATA[4][5]; olday = DATA[4][6]; oldaz = DATA[4][4]; double head = DATA[18][2] - 90; #if MAVLINK10 imu.time_usec = ((ulong)DateTime.Now.ToBinary()); #else imu.usec = ((ulong)DateTime.Now.ToBinary()); #endif imu.xgyro = xgyro; // roll - yes imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000); imu.ygyro = ygyro; // pitch - yes imu.ymag = (short)(Math.Cos(head * deg2rad) * 1000); imu.zgyro = zgyro; imu.zmag = 0; imu.xacc = (Int16)(accel3D.X * 1000); // pitch imu.yacc = (Int16)(accel3D.Y * 1000); // roll imu.zacc = (Int16)(accel3D.Z * 1000); //Console.WriteLine("ax " + imu.xacc + " ay " + imu.yacc + " az " + imu.zacc); #if MAVLINK10 gps.alt = (int)(DATA[20][2] * ft2m * 1000); gps.fix_type = 3; gps.cog = (ushort)(DATA[19][2] * 100); gps.lat = (int)(DATA[20][0] * 1.0e7); gps.lon = (int)(DATA[20][1] * 1.0e7); gps.time_usec = ((ulong)0); gps.vel = (ushort)(DATA[3][7] * 0.44704 * 100); #else gps.alt = ((float)(DATA[20][2] * ft2m)); gps.fix_type = 3; gps.hdg = ((float)DATA[19][2]); gps.lat = ((float)DATA[20][0]); gps.lon = ((float)DATA[20][1]); gps.usec = ((ulong)0); gps.v = ((float)(DATA[3][7] * 0.44704)); #endif asp.airspeed = ((float)(DATA[3][6] * 0.44704)); } else if (receviedbytes == 0x64) // FG binary udp { //FlightGear object imudata = new fgIMUData(); MAVLink.ByteArrayToStructureEndian(data, ref imudata, 0); imudata = (fgIMUData)(imudata); fgIMUData imudata2 = (fgIMUData)imudata; if (imudata2.magic != 0x4c56414d) return; if (imudata2.latitude == 0) return; chkSensor.Checked = true; #if MAVLINK10 imu.time_usec = ((ulong)DateTime.Now.ToBinary()); #else imu.usec = ((ulong)DateTime.Now.ToBinary()); #endif imu.xacc = ((Int16)(imudata2.accelX * 9808 / 32.2)); imu.xgyro = ((Int16)(imudata2.rateRoll * 17.453293)); imu.xmag = 0; imu.yacc = ((Int16)(imudata2.accelY * 9808 / 32.2)); imu.ygyro = ((Int16)(imudata2.ratePitch * 17.453293)); imu.ymag = 0; imu.zacc = ((Int16)(imudata2.accelZ * 9808 / 32.2)); // + 1000 imu.zgyro = ((Int16)(imudata2.rateYaw * 17.453293)); imu.zmag = 0; #if MAVLINK10 gps.alt = ((int)(imudata2.altitude * ft2m * 1000)); gps.fix_type = 3; gps.cog = (ushort)(Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg * 100); gps.lat = (int)(imudata2.latitude * 1.0e7); gps.lon = (int)(imudata2.longitude * 1.0e7); gps.time_usec = ((ulong)DateTime.Now.Ticks); gps.vel = (ushort)(Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m * 100); #else gps.alt = ((float)(imudata2.altitude * ft2m)); gps.fix_type = 3; gps.hdg = ((float)Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg); gps.lat = ((float)imudata2.latitude); gps.lon = ((float)imudata2.longitude); gps.usec = ((ulong)DateTime.Now.Ticks); gps.v = ((float)Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m); #endif //FileStream stream = File.OpenWrite("fgdata.txt"); //stream.Write(data, 0, receviedbytes); //stream.Close(); } else if (receviedbytes == 582) { aeroin = new TDataFromAeroSimRC(); object temp = aeroin; MAVLink.ByteArrayToStructure(data, ref temp, 0); aeroin = (TDataFromAeroSimRC)(temp); att.pitch = (aeroin.Model_fPitch); att.roll = (aeroin.Model_fRoll * -1); att.yaw = (float)((aeroin.Model_fHeading)); att.pitchspeed = (aeroin.Model_fAngVelX); att.rollspeed = (aeroin.Model_fAngVelY); att.yawspeed = (aeroin.Model_fAngVelZ); #if MAVLINK10 imu.time_usec = ((ulong)DateTime.Now.ToBinary()); #else imu.usec = ((ulong)DateTime.Now.ToBinary()); #endif imu.xgyro = (short)(aeroin.Model_fAngVelX * 1000); // roll - yes //imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000); imu.ygyro = (short)(aeroin.Model_fAngVelY * 1000); // pitch - yes //imu.ymag = (short)(Math.Cos(head * deg2rad) * 1000); imu.zgyro = (short)(aeroin.Model_fAngVelZ * 1000); //imu.zmag = 0; YLScsDrawing.Drawing3d.Vector3d accel3D = HIL.QuadCopter.RPY_to_XYZ(att.roll, att.pitch, 0, -9.8); //DATA[18][2] imu.xacc = (Int16)((accel3D.X + aeroin.Model_fAccelX) * 1000); // pitch imu.yacc = (Int16)((accel3D.Y + aeroin.Model_fAccelY) * 1000); // roll imu.zacc = (Int16)((accel3D.Z + aeroin.Model_fAccelZ) * 1000); Console.WriteLine("x {0} y {1} z {2}", imu.xacc, imu.yacc, imu.zacc); #if MAVLINK10 gps.alt = ((int)(aeroin.Model_fPosZ) * 1000); gps.fix_type = 3; gps.cog = (ushort)(Math.Atan2(aeroin.Model_fVelX, aeroin.Model_fVelY) * rad2deg * 100); gps.lat = (int)(aeroin.Model_fLatitude * 1.0e7); gps.lon = (int)(aeroin.Model_fLongitude * 1.0e7); gps.time_usec = ((ulong)DateTime.Now.Ticks); gps.vel = (ushort)(Math.Sqrt((aeroin.Model_fVelY * aeroin.Model_fVelY) + (aeroin.Model_fVelX * aeroin.Model_fVelX)) * 100); #else gps.alt = ((float)(aeroin.Model_fPosZ)); gps.fix_type = 3; gps.hdg = ((float)Math.Atan2(aeroin.Model_fVelX, aeroin.Model_fVelY) * rad2deg); gps.lat = ((float)aeroin.Model_fLatitude); gps.lon = ((float)aeroin.Model_fLongitude); gps.usec = ((ulong)DateTime.Now.Ticks); gps.v = ((float)Math.Sqrt((aeroin.Model_fVelY * aeroin.Model_fVelY) + (aeroin.Model_fVelX * aeroin.Model_fVelX))); #endif float xvec = aeroin.Model_fVelY - aeroin.Model_fWindVelY; float yvec = aeroin.Model_fVelX - aeroin.Model_fWindVelX; asp.airspeed = ((float)Math.Sqrt((yvec * yvec) + (xvec * xvec))); } else if (receviedbytes > 0x100) { FGNetFDM fdm = new FGNetFDM(); object temp = fdm; MAVLink.ByteArrayToStructureEndian(data, ref temp, 0); fdm = (FGNetFDM)(temp); lastfdmdata = fdm; att.roll = fdm.phi; att.pitch = fdm.theta; att.yaw = fdm.psi; #if MAVLINK10 imu.time_usec = ((ulong)DateTime.Now.ToBinary()); #else imu.usec = ((ulong)DateTime.Now.ToBinary()); #endif imu.xgyro = (short)(fdm.phidot * 1150); // roll - yes //imu.xmag = (short)(Math.Sin(head * deg2rad) * 1000); imu.ygyro = (short)(fdm.thetadot * 1150); // pitch - yes //imu.ymag = (short)(Math.Cos(head * deg2rad) * 1000); imu.zgyro = (short)(fdm.psidot * 1150); imu.zmag = 0; imu.xacc = (Int16)Math.Min(Int16.MaxValue, Math.Max(Int16.MinValue, (fdm.A_X_pilot * 9808 / 32.2))); // pitch imu.yacc = (Int16)Math.Min(Int16.MaxValue, Math.Max(Int16.MinValue, (fdm.A_Y_pilot * 9808 / 32.2))); // roll imu.zacc = (Int16)Math.Min(Int16.MaxValue, Math.Max(Int16.MinValue, (fdm.A_Z_pilot / 32.2 * 9808))); //Console.WriteLine("ax " + imu.xacc + " ay " + imu.yacc + " az " + imu.zacc); #if MAVLINK10 gps.alt = ((int)(fdm.altitude * ft2m * 1000)); gps.fix_type = 3; gps.cog = (ushort)((((Math.Atan2(fdm.v_east, fdm.v_north) * rad2deg) + 360) % 360) * 100); gps.lat = (int)(fdm.latitude * rad2deg * 1.0e7); gps.lon = (int)(fdm.longitude * rad2deg * 1.0e7); gps.time_usec = ((ulong)DateTime.Now.Ticks); gps.vel = (ushort)(Math.Sqrt((fdm.v_north * fdm.v_north) + (fdm.v_east * fdm.v_east)) * ft2m * 100); #else gps.alt = ((float)(fdm.altitude * ft2m)); gps.fix_type = 3; gps.hdg = (float)(((Math.Atan2(fdm.v_east, fdm.v_north) * rad2deg) + 360) % 360); //Console.WriteLine(gps.hdg); gps.lat = ((float)fdm.latitude * rad2deg); gps.lon = ((float)fdm.longitude * rad2deg); gps.usec = ((ulong)DateTime.Now.Ticks); gps.v = ((float)Math.Sqrt((fdm.v_north * fdm.v_north) + (fdm.v_east * fdm.v_east)) * ft2m); #endif asp.airspeed = fdm.vcas * kts2fps * ft2m; } else { //FlightGear - old style udp DATA[20] = new float[8]; DATA[18] = new float[8]; DATA[19] = new float[8]; DATA[3] = new float[8]; // this text line is defined from ardupilot.xml string telem = Encoding.ASCII.GetString(data, 0, data.Length); try { // should convert this to regex.... or just leave it. int oldpos = 0; int pos = telem.IndexOf(","); DATA[20][0] = float.Parse(telem.Substring(oldpos, pos - 1), new System.Globalization.CultureInfo("en-US")); oldpos = pos; pos = telem.IndexOf(",", pos + 1); DATA[20][1] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US")); oldpos = pos; pos = telem.IndexOf(",", pos + 1); DATA[20][2] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US")); oldpos = pos; pos = telem.IndexOf(",", pos + 1); DATA[18][1] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US")); oldpos = pos; pos = telem.IndexOf(",", pos + 1); DATA[18][0] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US")); oldpos = pos; pos = telem.IndexOf(",", pos + 1); DATA[19][2] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US")); oldpos = pos; pos = telem.IndexOf("\n", pos + 1); DATA[3][6] = float.Parse(telem.Substring(oldpos + 1, pos - 1 - oldpos), new System.Globalization.CultureInfo("en-US")); DATA[3][7] = DATA[3][6]; } catch (Exception) { } chkSensor.Checked = false; att.pitch = (DATA[18][0]); att.roll = (DATA[18][1]); att.yaw = (DATA[19][2]); #if MAVLINK10 gps.alt = ((int)(DATA[20][2] * ft2m * 1000)); gps.fix_type = 3; gps.cog = (ushort)(DATA[18][2] * 100); gps.lat = (int)(DATA[20][0] * 1.0e7); gps.lon = (int)(DATA[20][1] * 1.0e7); gps.time_usec = ((ulong)0); gps.vel = (ushort)((DATA[3][7] * 0.44704 * 100)); #else gps.alt = ((float)(DATA[20][2] * ft2m)); gps.fix_type = 3; gps.hdg = ((float)DATA[18][2]); gps.lat = ((float)DATA[20][0]); gps.lon = ((float)DATA[20][1]); gps.usec = ((ulong)0); gps.v = ((float)(DATA[3][7] * 0.44704)); #endif asp.airspeed = ((float)(DATA[3][6] * 0.44704)); } // write arduimu to ardupilot if (CHK_quad.Checked) // quad does its own { return; } #if MAVLINK10 TimeSpan gpsspan = DateTime.Now - lastgpsupdate; if (gpsspan.TotalMilliseconds >= GPS_rate) { lastgpsupdate = DateTime.Now; oldgps = gps; //comPort.sendPacket(gps); } hilstate.alt = oldgps.alt; hilstate.lat = oldgps.lat; hilstate.lon = oldgps.lon; hilstate.pitch = att.pitch; hilstate.pitchspeed = att.pitchspeed; hilstate.roll = att.roll; hilstate.rollspeed = att.rollspeed; hilstate.time_usec = gps.time_usec; hilstate.vx = (short)(gps.vel * Math.Sin(oldgps.cog / 100.0 * deg2rad)); hilstate.vy = (short)(gps.vel * Math.Cos(oldgps.cog / 100.0 * deg2rad)); hilstate.vz = 0; hilstate.xacc = imu.xacc; hilstate.yacc = imu.yacc; hilstate.yaw = att.yaw; hilstate.yawspeed = att.yawspeed; hilstate.zacc = imu.zacc; comPort.sendPacket(hilstate); comPort.sendPacket(asp); #else if (chkSensor.Checked == false) // attitude { comPort.sendPacket( att); comPort.sendPacket( asp); } else // raw imu { // imudata comPort.sendPacket( imu); #endif MAVLink.__mavlink_raw_pressure_t pres = new MAVLink.__mavlink_raw_pressure_t(); double calc = (101325 * Math.Pow(1 - 2.25577 * Math.Pow(10, -5) * gps.alt, 5.25588)); // updated from valid gps pres.press_diff1 = (short)(int)(calc - 101325); // 0 alt is 0 pa comPort.sendPacket(pres); #if !MAVLINK10 comPort.sendPacket(asp); } TimeSpan gpsspan = DateTime.Now - lastgpsupdate; if (gpsspan.TotalMilliseconds >= GPS_rate) { lastgpsupdate = DateTime.Now; comPort.sendPacket(gps); } #endif }
public void update(ref double[] servos, ArdupilotMega.GCSViews.Simulation.FGNetFDM fdm) { for (int i = 0; i < servos.Length; i++) { if (servos[i] <= 0.0) { motor_speed[i] = 0; } else { motor_speed[i] = scale_rc(i, (float)servos[i], 0.0f, 1.0f); //servos[i] = motor_speed[i]; } } double[] m = motor_speed; /* roll = 0; pitch = 0; yaw = 0; roll_rate = 0; pitch_rate = 0; yaw_rate = 0; */ // Console.WriteLine("\nin m {0:0.000000} {1:0.000000} {2:0.000000} {3:0.000000}", m[0], m[1], m[2], m[3]); // Console.WriteLine("in vel {0:0.000000} {1:0.000000} {2:0.000000}", velocity.X, velocity.Y, velocity.Z); //Console.WriteLine("in r {0:0.000000} p {1:0.000000} y {2:0.000000} - r {3:0.000000} p {4:0.000000} y {5:0.000000}", roll, pitch, yaw, roll_rate, pitch_rate, yaw_rate); // m[0] *= 0.5; //# how much time has passed? DateTime t = DateTime.Now; TimeSpan delta_time = t - last_time; // 0.02 last_time = t; if (delta_time.TotalMilliseconds > 100) // somethings wrong / debug { delta_time = new TimeSpan(0, 0, 0, 0, 20); } //# rotational acceleration, in degrees/s/s double roll_accel = (m[1] - m[0]) * 5000.0; double pitch_accel = (m[2] - m[3]) * 5000.0; double yaw_accel = -((m[2] + m[3]) - (m[0] + m[1])) * 400.0; //Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll); //# update rotational rates roll_rate += roll_accel * delta_time.TotalSeconds; pitch_rate += pitch_accel * delta_time.TotalSeconds; yaw_rate += yaw_accel * delta_time.TotalSeconds; // Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll); //# update rotation roll += roll_rate * delta_time.TotalSeconds; pitch += pitch_rate * delta_time.TotalSeconds; yaw += yaw_rate * delta_time.TotalSeconds; // Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll); //Console.WriteLine("r {0:0.0} p {1:0.0} y {2:0.0} - r {3:0.0} p {4:0.0} y {5:0.0} ms {6:0.000}", roll, pitch, yaw, roll_rate, pitch_rate, yaw_rate, delta_time.TotalSeconds); //# air resistance Vector3d air_resistance = -velocity * (gravity / terminal_velocity); //# normalise rotations normalise(); double thrust = (m[0] + m[1] + m[2] + m[3]) * thrust_scale;//# Newtons double accel = thrust / mass; //Console.WriteLine("in {0:0.000000} {1:0.000000} {2:0.000000} {3:0.000000}", roll, pitch, yaw, accel); Vector3d accel3D = RPY_to_XYZ(roll, pitch, yaw, accel); //Console.WriteLine("accel3D " + accel3D.X + " " + accel3D.Y + " " + accel3D.Z); accel3D += new Vector3d(0, 0, -gravity); accel3D += air_resistance; Random rand = new Random(); int fixme; //velocity.X += .02 + rand.NextDouble() * .03; //velocity.Y += .02 + rand.NextDouble() * .03; //# new velocity vector velocity += accel3D * delta_time.TotalSeconds; this.accel = accel3D; //# new position vector old_position = new Vector3d(position); position += velocity * delta_time.TotalSeconds; // Console.WriteLine(fdm.agl + " "+ fdm.altitude); //Console.WriteLine("Z {0} halt {1} < gl {2} fh {3}" ,position.Z , home_altitude , ground_level , frame_height); if (home_latitude == 0 || home_latitude > 90 || home_latitude < -90 || home_longitude == 0) { this.home_latitude = fdm.latitude * rad2deg; this.home_longitude = fdm.longitude * rad2deg; this.home_altitude = fdm.altitude * ft2m; this.ground_level = this.home_altitude; this.altitude = fdm.altitude * ft2m; this.latitude = fdm.latitude * rad2deg; this.longitude = fdm.longitude * rad2deg; } //# constrain height to the ground if (position.Z + home_altitude < ground_level + frame_height) { if (old_position.Z + home_altitude > ground_level + frame_height) { // Console.WriteLine("Hit ground at {0} m/s", (-velocity.Z)); } velocity = new Vector3d(0, 0, 0); roll_rate = 0; pitch_rate = 0; yaw_rate = 0; roll = 0; pitch = 0; position = new Vector3d(position.X, position.Y, ground_level + frame_height - home_altitude + 0); // Console.WriteLine("here " + position.Z); } //# update lat/lon/altitude update_position(); // send to apm #if MAVLINK10 ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_int_t(); #else ArdupilotMega.MAVLink.__mavlink_gps_raw_t gps = new ArdupilotMega.MAVLink.__mavlink_gps_raw_t(); #endif ArdupilotMega.MAVLink.__mavlink_attitude_t att = new ArdupilotMega.MAVLink.__mavlink_attitude_t(); ArdupilotMega.MAVLink.__mavlink_vfr_hud_t asp = new ArdupilotMega.MAVLink.__mavlink_vfr_hud_t(); att.roll = (float)roll * deg2rad; att.pitch = (float)pitch * deg2rad; att.yaw = (float)yaw * deg2rad; att.rollspeed = (float)roll_rate * deg2rad; att.pitchspeed = (float)pitch_rate * deg2rad; att.yawspeed = (float)yaw_rate * deg2rad; #if MAVLINK10 gps.alt = ((int)(altitude * 1000)); gps.fix_type = 3; gps.vel = (ushort)(Math.Sqrt((velocity.X * velocity.X) + (velocity.Y * velocity.Y)) * 100); gps.cog = (ushort)((((Math.Atan2(velocity.Y, velocity.X) * rad2deg) + 360) % 360) * 100); gps.lat = (int)(latitude* 1.0e7); gps.lon = (int)(longitude * 1.0e7); gps.time_usec = ((ulong)DateTime.Now.Ticks); asp.airspeed = gps.vel; #else gps.alt = ((float)(altitude)); gps.fix_type = 3; gps.lat = ((float)latitude); gps.lon = ((float)longitude); gps.usec = ((ulong)DateTime.Now.Ticks); //Random rand = new Random(); //gps.alt += (rand.Next(100) - 50) / 100.0f; //gps.lat += (float)((rand.NextDouble() - 0.5) * 0.00005); //gps.lon += (float)((rand.NextDouble() - 0.5) * 0.00005); //gps.v += (float)(rand.NextDouble() - 0.5) * 1.0f; gps.v = ((float)Math.Sqrt((velocity.X * velocity.X) + (velocity.Y * velocity.Y))); gps.hdg = (float)(((Math.Atan2(velocity.Y, velocity.X) * rad2deg) + 360) % 360); ; asp.airspeed = gps.v; #endif MainV2.comPort.sendPacket(att); MAVLink.__mavlink_raw_pressure_t pres = new MAVLink.__mavlink_raw_pressure_t(); double calc = (101325 * Math.Pow(1 - 2.25577 * Math.Pow(10, -5) * gps.alt, 5.25588)); pres.press_diff1 = (short)(int)(calc); // 0 alt is 0 pa MainV2.comPort.sendPacket(pres); MainV2.comPort.sendPacket(asp); if (framecount % 12 == 0) {// 50 / 10 = 5 hz MainV2.comPort.sendPacket(gps); //Console.WriteLine(DateTime.Now.Millisecond + " GPS" ); } framecount++; }