mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 22:48:29 -04:00
529e1bcc71
add enable/disable to mavlinkcheckbox modify my button to curved add delay to progress reporter dialog. to ensure correct parent Fix Mount screen for AP Fix Hardware screen Text display roi difrently modify HIL/Quad Hil update dataflashlog format (thanks randy) update mavcmd format for roi
365 lines
11 KiB
C#
365 lines
11 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using Sharp3D.Math.Core;
|
|
|
|
namespace ArdupilotMega.HIL
|
|
{
|
|
class FlashQuad
|
|
{
|
|
public ahrsc ahrs = new ahrsc() ;// :AHRS;
|
|
public Vector3D loc = new Vector3D() ;// :Location;
|
|
public param g = new param() ;// :Parameters;
|
|
//public var apm_rc :APM_RC;
|
|
//public var motor_filter_0 :AverageFilter;
|
|
//public var motor_filter_1 :AverageFilter;
|
|
//public var motor_filter_2 :AverageFilter;
|
|
//public var motor_filter_3 :AverageFilter;
|
|
public Vector3D drag = new Vector3D();// :Vector3D; //
|
|
public Vector3D airspeed = new Vector3D();// :Vector3D; //
|
|
//public var thrust :Vector3D; //
|
|
public Vector3D position = new Vector3D();// :Vector3D; //
|
|
public Vector3D velocity = new Vector3D();// :Vector3D;
|
|
//public var velocity_old :Vector3D;
|
|
//public var wind :Point; //
|
|
public Vector3D rot_accel = new Vector3D();// :Vector3D; //
|
|
public Vector3D angle3D = new Vector3D();// :Vector3D; //
|
|
//public var windGenerator :Wind; //
|
|
|
|
public double gravity = 980.5;
|
|
public double thrust_scale = 0.4;
|
|
public double throttle = 500;
|
|
public double rotation_bias = 1;
|
|
|
|
private double _jump_z = 0;
|
|
private Vector3D v3 = new Vector3D();
|
|
|
|
public class ahrsc
|
|
{
|
|
public Matrix3D dcm = new Matrix3D(Matrix3D.Identity);
|
|
public Vector3D gyro = new Vector3D();
|
|
public Vector3D omega = new Vector3D();
|
|
public Vector3D accel = new Vector3D();
|
|
public Vector3D rotation_speed = new Vector3D();
|
|
|
|
public double roll_sensor;
|
|
public double pitch_sensor;
|
|
public double yaw_sensor;
|
|
}
|
|
|
|
public class param
|
|
{
|
|
// ---------------------------------------------
|
|
// Sim Details controls
|
|
// ---------------------------------------------
|
|
public int sim_iterations = 99999;
|
|
public int sim_speed = 1;
|
|
|
|
public double windSpeedMin = 150;
|
|
public double windSpeedMax = 200;
|
|
public double windPeriod = 30000;
|
|
public double windDir = 45;
|
|
|
|
public double airDensity = 1.184;
|
|
//public var crossSection :Number = 0.015;
|
|
public double crossSection = 0.012;
|
|
public double dragCE = 0.20;
|
|
public double speed_filter_size = 2;
|
|
public double motor_kv = 1000;
|
|
public double moment = 3;
|
|
public double mass = 500;
|
|
public int esc_delay = 12;
|
|
|
|
// -----------------------------------------
|
|
// Inertial control
|
|
// -----------------------------------------
|
|
public double speed_correction_z = 0.0350;
|
|
public double xy_speed_correction = 0.030;
|
|
public double xy_offset_correction = 0.00001;
|
|
public double xy_pos_correction = 0.08;
|
|
|
|
public double z_offset_correction = 0.00004;
|
|
public double z_pos_correction = 0.2;
|
|
|
|
public double accel_bias_x = .9;
|
|
public double accel_bias_z = .9;
|
|
public double accel_bias_y = .9;
|
|
}
|
|
|
|
double scale_rc(int sn, float servo, float min, float max)
|
|
{
|
|
float min_pwm = 1000;
|
|
float max_pwm = 2000;
|
|
//'''scale a PWM value''' # default to servo range of 1000 to 2000
|
|
if (MainV2.comPort.param.Count > 0)
|
|
{
|
|
min_pwm = int.Parse(MainV2.comPort.param["RC3_MIN"].ToString());
|
|
max_pwm = int.Parse(MainV2.comPort.param["RC3_MAX"].ToString());
|
|
}
|
|
|
|
float p = (servo - min_pwm) / (max_pwm - min_pwm);
|
|
|
|
float v = min + p * (max - min);
|
|
|
|
if (v < min)
|
|
v = min;
|
|
if (v > max)
|
|
v = max;
|
|
return v * 1000;
|
|
}
|
|
|
|
public void update(ref double[] motor_output, double dt)
|
|
{
|
|
var _thrust = 0.0;
|
|
rot_accel = new Vector3D(0,0,0);
|
|
angle3D.X = angle3D.Y = 0;
|
|
angle3D.Z = 1;
|
|
|
|
// wind = windGenerator.read();
|
|
|
|
// ESC's moving average filter
|
|
// var motor_output:Array = new Array(4);
|
|
// motor_output[0] = motor_filter_0.apply(apm_rc.get_motor_output(0));
|
|
// motor_output[1] = motor_filter_1.apply(apm_rc.get_motor_output(1));
|
|
// motor_output[2] = motor_filter_2.apply(apm_rc.get_motor_output(2));
|
|
// motor_output[3] = motor_filter_3.apply(apm_rc.get_motor_output(3));
|
|
|
|
|
|
for (int i = 0; i < motor_output.Length; i++)
|
|
{
|
|
if (motor_output[i] <= 0.0)
|
|
{
|
|
motor_output[i] = 0;
|
|
}
|
|
else
|
|
{
|
|
motor_output[i] = scale_rc(i, (float)motor_output[i], 0.0f, 1.0f);
|
|
//servos[i] = motor_speed[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
2
|
|
|
|
1 0
|
|
|
|
3
|
|
|
|
*/
|
|
|
|
// setup motor rotations
|
|
rot_accel.X -= g.motor_kv * motor_output[0]; // roll
|
|
rot_accel.X += g.motor_kv * motor_output[1];
|
|
rot_accel.Y -= g.motor_kv * motor_output[3];
|
|
rot_accel.Y += g.motor_kv * motor_output[2];
|
|
|
|
rot_accel.Z += g.motor_kv * motor_output[0] * .08; // YAW
|
|
rot_accel.Z += g.motor_kv * motor_output[1] * .08;
|
|
rot_accel.Z -= g.motor_kv * motor_output[2] * .08;
|
|
rot_accel.Z -= g.motor_kv * motor_output[3] * .08;
|
|
|
|
rot_accel.X /= g.moment;
|
|
rot_accel.Y /= g.moment;
|
|
rot_accel.Z /= g.moment;
|
|
|
|
//# rotational air resistance
|
|
|
|
// Gyro is the rotation speed in deg/s
|
|
// update rotational rates in body frame
|
|
ahrs.gyro.X += rot_accel.X * dt;
|
|
ahrs.gyro.Y += rot_accel.Y * dt;
|
|
ahrs.gyro.Z += rot_accel.Z * dt;
|
|
|
|
//ahrs.gyro.z += 200;
|
|
ahrs.gyro.Z *= .995;// some drag
|
|
|
|
// ahrs.dcm = Matrix3D.Identity;
|
|
|
|
// move earth frame to body frame
|
|
Vector3D tmp = Matrix3D.Transform(ahrs.dcm,ahrs.gyro) ;//ahrs.dcm.transformVector(ahrs.gyro);
|
|
|
|
// update attitude:
|
|
ahrs.dcm += new Matrix3D(new Vector3D((tmp.X/100) * dt,0,0),new Vector3D(0,(tmp.Y/100) * dt,0),new Vector3D(0,0,(tmp.Z/100) * dt));
|
|
|
|
//ahrs.dcm.appendRotation((tmp.X/100) * dt, Vector3D.X_AXIS); // ROLL
|
|
//ahrs.dcm.appendRotation((tmp.Y/100) * dt, Vector3D.Y_AXIS); // PITCH
|
|
//ahrs.dcm.appendRotation((tmp.Z/100) * dt, Vector3D.Z_AXIS); // YAW
|
|
|
|
// ------------------------------------
|
|
// calc thrust
|
|
// ------------------------------------
|
|
|
|
//get_motor_output returns 0 : 1000
|
|
_thrust += motor_output[0] * thrust_scale;
|
|
_thrust += motor_output[1] * thrust_scale;
|
|
_thrust += motor_output[2] * thrust_scale;
|
|
_thrust += motor_output[3] * thrust_scale;
|
|
|
|
Vector3D accel_body = new Vector3D(0, 0, (_thrust * -.9) / g.mass);
|
|
//var accel_body:Vector3D = new Vector3D(0, 0, 0);
|
|
Vector3D accel_earth = Matrix3D.Transform(ahrs.dcm,(accel_body));
|
|
angle3D = Matrix3D.Transform(ahrs.dcm,(angle3D));
|
|
|
|
//trace(ahrs.gyro.y, accel_earth.x);
|
|
|
|
//trace(ahrs.gyro.x, ahrs.gyro.y, ahrs.gyro.z);
|
|
|
|
// ------------------------------------
|
|
// calc copter velocity
|
|
// ------------------------------------
|
|
// calc Drag
|
|
drag.X = .5 * g.airDensity * airspeed.X * airspeed.X * g.dragCE * g.crossSection;
|
|
drag.Y = .5 * g.airDensity * airspeed.Y * airspeed.Y * g.dragCE * g.crossSection;
|
|
drag.Z = .5 * g.airDensity * airspeed.Z * airspeed.Z * g.dragCE * g.crossSection;
|
|
|
|
///*
|
|
// this calulation includes wind
|
|
if(airspeed.X >= 0)
|
|
accel_earth.X -= drag.X;
|
|
else
|
|
accel_earth.X += drag.X;
|
|
|
|
// Add in Drag
|
|
if(airspeed.Y >= 0)
|
|
accel_earth.Y -= drag.Y;
|
|
else
|
|
accel_earth.Y += drag.Y;
|
|
|
|
if(airspeed.Z <= 0)
|
|
accel_earth.Z -= drag.Z;
|
|
else
|
|
accel_earth.Z += drag.Z;
|
|
//*/
|
|
|
|
// hacked vert disturbance
|
|
accel_earth.Z += _jump_z * dt;
|
|
_jump_z *= .999;
|
|
|
|
|
|
// Add in Gravity
|
|
accel_earth.Z += gravity;
|
|
|
|
if(accel_earth.Z < 0)
|
|
accel_earth.Z *=.9;
|
|
|
|
|
|
if(position.Z <=.11 && accel_earth.Z > 0){
|
|
accel_earth.Z = 0;
|
|
}
|
|
|
|
velocity.X += (accel_earth.X * dt); // + : Forward (North)
|
|
velocity.Y += (accel_earth.Y * dt); // + : Right (East)
|
|
velocity.Z -= (accel_earth.Z * dt); // + : Up
|
|
|
|
|
|
//trace(Math.floor(velocity.x),Math.floor(velocity.y),Math.floor(velocity.z));
|
|
|
|
// ------------------------------------
|
|
// calc inertia
|
|
// ------------------------------------
|
|
|
|
// work out acceleration as seen by the accelerometers. It sees the kinematic
|
|
// acceleration (ie. real movement), plus gravity
|
|
Matrix3D dcm_t = ahrs.dcm.Clone();
|
|
dcm_t.Transpose();
|
|
double t = accel_earth.Z;
|
|
accel_earth.Z -= gravity;
|
|
ahrs.accel = Matrix3D.Transform(dcm_t,(accel_earth));
|
|
|
|
ahrs.accel *= 0.01;
|
|
|
|
//ahrs.accel = accel_earth.clone();
|
|
ahrs.accel.X *= g.accel_bias_x;
|
|
ahrs.accel.Y *= g.accel_bias_y;
|
|
ahrs.accel.Z *= g.accel_bias_z;
|
|
|
|
|
|
|
|
// ------------------------------------
|
|
// calc Position
|
|
// ------------------------------------
|
|
position.Y += velocity.X * dt;
|
|
position.X += velocity.Y * dt;
|
|
position.Z += velocity.Z * dt;
|
|
position.Z = Math.Min(position.Z, 4000);
|
|
|
|
// XXX Force us to 3m above ground
|
|
//position.z = 300;
|
|
|
|
airspeed.X = (velocity.X);// - wind.x);
|
|
airspeed.Y = (velocity.Y);// - wind.y);
|
|
airspeed.Z = velocity.Z;
|
|
|
|
// Altitude
|
|
// --------
|
|
if(position.Z <=.1){
|
|
position.Z = .1;
|
|
velocity.X = 0;
|
|
velocity.Y = 0;
|
|
velocity.Z = 0;
|
|
airspeed.X = 0;
|
|
airspeed.Y = 0;
|
|
airspeed.Z = 0;
|
|
//ahrs.init();
|
|
}
|
|
|
|
|
|
// get omega - the simulated Gyro output
|
|
ahrs.omega.X = radiansx100(ahrs.gyro.X);
|
|
ahrs.omega.Y = radiansx100(ahrs.gyro.Y);
|
|
ahrs.omega.Z = radiansx100(ahrs.gyro.Z);
|
|
|
|
// get the Eulers output
|
|
v3 = new Vector3D(Math.Atan2(-ahrs.dcm.M23, ahrs.dcm.M22), Math.Asin(ahrs.dcm.M21), Math.Atan2(-ahrs.dcm.M31, ahrs.dcm.M11));// ahrs.dcm.decompose();
|
|
ahrs.roll_sensor = Math.Floor(degrees(v3.X) * 100);
|
|
ahrs.pitch_sensor = Math.Floor(degrees(v3.Y) * 100);
|
|
ahrs.yaw_sensor = Math.Floor(degrees(v3.Z) * 100);
|
|
|
|
// store the position for the GPS object
|
|
loc.X = position.X;
|
|
loc.Y = position.Y;
|
|
loc.Z = position.Z;
|
|
}
|
|
|
|
public double constrain(double val, double min, double max)
|
|
{
|
|
val = Math.Max(val, min);
|
|
val = Math.Min(val, max);
|
|
return val;
|
|
}
|
|
|
|
public double wrap_180(int error)
|
|
{
|
|
if (error > 18000) error -= 36000;
|
|
if (error < -18000) error += 36000;
|
|
return error;
|
|
}
|
|
|
|
public int wrap_360(int error)
|
|
{
|
|
if (error > 36000) error -= 36000;
|
|
if (error < 0) error += 36000;
|
|
return error;
|
|
}
|
|
|
|
public double radiansx100(double n)
|
|
{
|
|
return 0.000174532925 * n;
|
|
}
|
|
|
|
public double degreesx100(double r)
|
|
{
|
|
return r * 5729.57795;
|
|
}
|
|
public double degrees(double r)
|
|
{
|
|
return r * 57.2957795;
|
|
}
|
|
public double radians(double n)
|
|
{
|
|
return 0.0174532925 * n;
|
|
}
|
|
}
|
|
}
|