mirror of https://github.com/ArduPilot/ardupilot
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;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|