diff --git a/libraries/SITL/SIM_Blimp.cpp b/libraries/SITL/SIM_Blimp.cpp index ea129152b9..c5f8835b85 100644 --- a/libraries/SITL/SIM_Blimp.cpp +++ b/libraries/SITL/SIM_Blimp.cpp @@ -17,6 +17,7 @@ */ #include "SIM_Blimp.h" +#include #include @@ -30,17 +31,21 @@ Blimp::Blimp(const char *frame_str) : mass = 0.07; radius = 0.25; moment_of_inertia = {0.004375, 0.004375, 0.004375}; //m*r^2 for hoop... - k_tan = 5.52e-4; //Tangential (thrust) multiplier + cog = {0, 0, 0.1}; //10 cm down from center (i.e. center of buoyancy), for now + k_tan = 0.6e-7; //Tangential (thrust) and normal force multipliers + k_nor = 0;//3.4e-7; drag_constant = 0.05; - drag_gyr_constant = 0.08; + drag_gyr_constant = 0.15; lock_step_scheduled = true; - ::printf("Starting Blimp AirFish model...\n"); + ::printf("Starting Blimp model\n"); } // calculate rotational and linear accelerations -void Blimp::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel) +void Blimp::calculate_forces(const struct sitl_input &input, Vector3f &body_acc, Vector3f &rot_accel) { + float delta_time = frame_time_us * 1.0e-6f; + if (!hal.scheduler->is_system_initialized()) { return; } @@ -48,55 +53,131 @@ void Blimp::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel //all fin setup for (uint8_t i=0; i<4; i++) { fin[i].last_angle = fin[i].angle; - fin[i].angle = filtered_servo_angle(input, i)*radians(75.0f); //for servo range of -75 deg to +75 deg - + if (input.servos[i] == 0) { + fin[i].angle = 0; + fin[1].servo_angle = 0; + } else { + fin[i].angle = filtered_servo_angle(input, i)*radians(45.0f)+radians(13.5); //for servo range of -75 deg to +75 deg + fin[i].servo_angle = filtered_servo_angle(input, i); + } + if (fin[i].angle < fin[i].last_angle) fin[i].dir = 0; //thus 0 = "angle is reducing" else fin[i].dir = 1; - - fin[i].vel = (fin[i].angle - fin[i].last_angle)/delta_time; //rad/s - fin[i].vel = constrain_float(fin[i].vel, radians(-450), radians(450)); - fin[i].T = pow(fin[i].vel,2) * k_tan; - + + fin[i].vel = degrees(fin[i].angle - fin[i].last_angle)/delta_time; //Could also do multi-point derivative filter - DerivativeFilter.cpp + //deg/s (should really be rad/s, but that would require modifying k_tan, k_nor) + //all other angles should be in radians. + fin[i].vel = constrain_float(fin[i].vel, -450, 450); + fin[i].T = sq(fin[i].vel) * k_tan; + fin[i].N = sq(fin[i].vel) * k_nor; + if (fin[i].dir == 0) fin[i].N = -fin[1].N; //normal force flips when fin changes direction + fin[i].Fx = 0; fin[i].Fy = 0; fin[i].Fz = 0; } - //TODO: Add normal force calculations and include roll & pitch oscillation. //Back fin - fin[0].Fx = fin[0].T*cos(fin[0].angle); //causes forward movement - fin[0].Fz = fin[0].T*sin(fin[0].angle); //causes height change + fin[0].Fx = fin[0].T*cos(fin[0].angle);// + fin[0].N*sin(fin[0].angle); //causes forward movement + fin[0].Fz = fin[0].T*sin(fin[0].angle);// - fin[0].N*cos(fin[0].angle); //causes height & wobble in y //Front fin - fin[1].Fx = -fin[1].T*cos(fin[1].angle); //causes backward movement - fin[1].Fz = fin[1].T*sin(fin[1].angle); //causes height change + fin[1].Fx = -fin[1].T*cos(fin[1].angle);// - fin[1].N*sin(fin[1].angle); //causes backward movement + fin[1].Fz = fin[1].T*sin(fin[1].angle);// - fin[1].N*cos(fin[1].angle); //causes height & wobble in y //Right fin - fin[2].Fy = -fin[2].T*cos(fin[2].angle); //causes left movement - fin[2].Fx = fin[2].T*sin(fin[2].angle); //causes yaw + fin[2].Fy = -fin[2].T*cos(fin[2].angle);// - fin[2].N*sin(fin[2].angle); //causes left movement + fin[2].Fx = fin[2].T*sin(fin[2].angle);// - fin[2].N*cos(fin[2].angle); //cause yaw & wobble in z //Left fin - fin[3].Fy = fin[3].T*cos(fin[3].angle); //causes right movement - fin[3].Fx = -fin[3].T*sin(fin[3].angle); //causes yaw + fin[3].Fy = fin[3].T*cos(fin[3].angle);// + fin[3].N*sin(fin[3].angle); //causes right movement + fin[3].Fx = fin[3].T*sin(fin[3].angle);// + fin[3].N*cos(fin[3].angle); //causes yaw & wobble in z - Vector3f force_bf{0,0,0}; + Vector3f F_BF{0,0,0}; for (uint8_t i=0; i<4; i++) { - force_bf.x = force_bf.x + fin[i].Fx; - force_bf.y = force_bf.y + fin[i].Fy; - force_bf.z = force_bf.z + fin[i].Fz; + F_BF.x = F_BF.x + fin[i].Fx; + F_BF.y = F_BF.y + fin[i].Fy; + F_BF.z = F_BF.z + fin[i].Fz; } - //mass in kg, thus accel in m/s/s - body_accel.x = force_bf.x/mass; - body_accel.y = force_bf.y/mass; - body_accel.z = force_bf.z/mass; + body_acc.x = F_BF.x/mass; //mass in kg, thus accel in m/s/s + body_acc.y = F_BF.y/mass; + body_acc.z = F_BF.z/mass; Vector3f rot_T{0,0,0}; - rot_T.z = fin[2].Fx * radius + fin[3].Fx * radius;//in N*m (Torque = force * lever arm) + AP::logger().WriteStreaming("SFT", "TimeUS,f0,f1,f2,f3", + "Qffff", + AP_HAL::micros64(), + fin[0].T, fin[1].T, fin[2].T, fin[3].T); + AP::logger().WriteStreaming("SFN", "TimeUS,n0,n1,n2,n3", + "Qffff", + AP_HAL::micros64(), + fin[0].N, fin[1].N, fin[2].N, fin[3].N); + AP::logger().WriteStreaming("SBA1", "TimeUS,ax,ay,az", + "Qfff", + AP_HAL::micros64(), + body_acc.x, body_acc.y, body_acc.z); + AP::logger().WriteStreaming("SFA1", "TimeUS,f0,f1,f2,f3", + "Qffff", + AP_HAL::micros64(), + fin[0].angle, fin[1].angle, fin[2].angle, fin[3].angle); + AP::logger().WriteStreaming("SFAN", "TimeUS,f0,f1,f2,f3", + "Qffff", + AP_HAL::micros64(), + fin[0].servo_angle, fin[1].servo_angle, fin[2].servo_angle, fin[3].servo_angle); + AP::logger().WriteStreaming("SSAN", "TimeUS,f0,f1,f2,f3", + "QHHHH", + AP_HAL::micros64(), + input.servos[0], input.servos[1], input.servos[2], input.servos[3]); + AP::logger().WriteStreaming("SFV1", "TimeUS,f0,f1,f2,f3", + "Qffff", + AP_HAL::micros64(), + fin[0].vel, fin[1].vel, fin[2].vel, fin[3].vel); + AP::logger().WriteStreaming("SRT1", "TimeUS,rtx,rty,rtz", + "Qfff", + AP_HAL::micros64(), + rot_T.x, rot_T.y, rot_T.z); + +#if 0 //"Wobble" attempt + rot_T.y = fin[0].Fz * radius + fin[1].Fz * radius; + AP::logger().WriteStreaming("SRT2", "TimeUS,rtx,rty,rtz", + "Qfff", + AP_HAL::micros64(), + rot_T.x, rot_T.y, rot_T.z); + // the blimp has pendulum stability due to the centre of gravity being lower than the centre of buoyancy + Vector3f ang; //x,y,z correspond to roll, pitch, yaw. + dcm.to_euler(&ang.x, &ang.y, &ang.z); //rpy in radians + Vector3f ang_ef = dcm * ang; + rot_T.x -= mass*GRAVITY_MSS*sinf(M_PI-ang_ef.x)/cog.z; + rot_T.y -= mass*GRAVITY_MSS*sinf(M_PI-ang_ef.y)/cog.z; + AP::logger().WriteStreaming("SRT3", "TimeUS,rtx,rty,rtz", + "Qfff", + AP_HAL::micros64(), + rot_T.x, rot_T.y, rot_T.z); + AP::logger().WriteStreaming("SAN1", "TimeUS,anx,any,anz", + "Qfff", + AP_HAL::micros64(), + ang.x, ang.y, ang.z); + AP::logger().WriteStreaming("SAN2", "TimeUS,anx,any,anz", + "Qfff", + AP_HAL::micros64(), + ang_ef.x, ang_ef.y, ang_ef.z); + AP::logger().WriteStreaming("SAF1", "TimeUS,afx,afy,afz", + "Qfff", + AP_HAL::micros64(), + sinf(ang.x), sinf(ang.y), sinf(ang.z)); + AP::logger().WriteStreaming("SMGC", "TimeUS,m,g,cz", + "Qfff", + AP_HAL::micros64(), + mass, GRAVITY_MSS, cog.z); +#endif + + rot_T.z = fin[2].Fx * radius - fin[3].Fx * radius;//in N*m (Torque = force * lever arm) //rot accel = torque / moment of inertia - rot_accel.x = 0; - rot_accel.y = 0; + //Torque = moment force. + rot_accel.x = rot_T.x / moment_of_inertia.x; + rot_accel.y = rot_T.y / moment_of_inertia.y; rot_accel.z = rot_T.z / moment_of_inertia.z; } @@ -105,10 +186,10 @@ void Blimp::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel */ void Blimp::update(const struct sitl_input &input) { - delta_time = frame_time_us * 1.0e-6f; + float delta_time = frame_time_us * 1.0e-6f; Vector3f rot_accel = Vector3f(0,0,0); - calculate_forces(input, rot_accel, accel_body); + calculate_forces(input, accel_body, rot_accel); if (hal.scheduler->is_system_initialized()) { float gyr_sq = gyro.length_squared(); @@ -120,12 +201,23 @@ void Blimp::update(const struct sitl_input &input) } } +#if 0 + AP::logger().WriteStreaming("SBLM", "TimeUS,RAx,RAy,RAz", + "Qfff", + AP_HAL::micros64(), + rot_accel.x, rot_accel.y, rot_accel.z); +#endif + // update rotational rates in body frame gyro += rot_accel * delta_time; + gyro.x = constrain_float(gyro.x, -radians(2000.0f), radians(2000.0f)); gyro.y = constrain_float(gyro.y, -radians(2000.0f), radians(2000.0f)); gyro.z = constrain_float(gyro.z, -radians(2000.0f), radians(2000.0f)); + // Vector3f ang; //x,y,z correspond to roll, pitch, yaw. + // dcm.to_euler(&ang.x, &ang.y, &ang.z); //rpy in radians + // dcm.from_euler(0.0f, 0.0f, ang.z); // update attitude dcm.rotate(gyro * delta_time); dcm.normalize(); @@ -139,10 +231,10 @@ void Blimp::update(const struct sitl_input &input) accel_body += bf_drag_accel; } - // add lifting force exactly equal to gravity, for neutral buoyancy + // add lifting force exactly equal to gravity, for neutral buoyancy (buoyancy in ef) accel_body += dcm.transposed() * Vector3f(0,0,-GRAVITY_MSS); } - + Vector3f accel_earth = dcm * accel_body; accel_earth += Vector3f(0.0f, 0.0f, GRAVITY_MSS); //add gravity velocity_ef += accel_earth * delta_time; @@ -151,4 +243,6 @@ void Blimp::update(const struct sitl_input &input) update_position(); //updates the position from the Vector3f position time_advance(); update_mag_field_bf(); + rate_hz = sitl->loop_rate_hz; + } diff --git a/libraries/SITL/SIM_Blimp.h b/libraries/SITL/SIM_Blimp.h index 646b466264..042341c5a1 100644 --- a/libraries/SITL/SIM_Blimp.h +++ b/libraries/SITL/SIM_Blimp.h @@ -19,6 +19,8 @@ #pragma once #include "SIM_Aircraft.h" +// #include "SIM_Motor.h" +// #include "SIM_Frame.h" namespace SITL { @@ -26,9 +28,11 @@ struct Fins { float angle; float last_angle; + float servo_angle; bool dir; - float vel; // velocity, in rad/s + float vel; // velocity, in m/s float T; //Tangential (thrust) force, in Neutons + float N; //Normal force, in Newtons float Fx; //Fx,y,z = Force in bodyframe orientation at servo position, in Newtons float Fy; float Fz; @@ -54,15 +58,17 @@ protected: float mass; //kilograms float radius; //metres Vector3f moment_of_inertia; + Vector3f cog; //centre of gravity location relative to center of blimp //Airfish-specific variables Fins fin[4]; - float k_tan; //Tangential force multiplier + float k_tan; //Tangential and normal force multipliers + float k_nor; float drag_constant; float drag_gyr_constant; - float delta_time; void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel); + float sq(float a) {return pow(a,2);} }; }