mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-06 19:00:27 +08:00
432 lines
16 KiB
C++
432 lines
16 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
/*
|
|
Blimp simulator class
|
|
*/
|
|
|
|
#include "SIM_Blimp.h"
|
|
#include <AP_Logger/AP_Logger.h>
|
|
|
|
#include <stdio.h>
|
|
|
|
using namespace SITL;
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
Blimp::Blimp(const char *frame_str) :
|
|
Aircraft(frame_str)
|
|
{
|
|
mass = 0.07; //i.e. 70g
|
|
radius = 0.25; // i.e. 25 cm
|
|
moment_of_inertia = {0.004375, 0.004375, 0.004375}; //m*r^2 for hoop...
|
|
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 for the fins
|
|
k_nor = 0;
|
|
k_m = 0.15; //Thrust multiplier for motors
|
|
gondolawidth = 0.1; //10 cm
|
|
drag_constant = 0.04;
|
|
drag_gyr_constant = 0.15;
|
|
// linear drag dominates at low speed/rate; without it quadratic-only
|
|
// drag decays as 1/t and the vehicle never quite stops moving/yawing
|
|
drag_lin_constant = 0.008;
|
|
drag_gyr_lin_constant = 0.035;
|
|
|
|
lock_step_scheduled = true;
|
|
|
|
constexpr float default_battery_resistance_ohm = 0.01f;
|
|
battery.setup(sitl->batt_capacity_ah,
|
|
default_battery_resistance_ohm,
|
|
sitl->batt_voltage,
|
|
ambient_outside_temperature_degC());
|
|
|
|
::printf("Starting Blimp model\n");
|
|
|
|
if (strstr(frame_str, "motor")) {
|
|
motorblimp = true;
|
|
::printf("Running motorblimp frame.\n");
|
|
}
|
|
}
|
|
|
|
// calculate rotational and linear accelerations
|
|
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;
|
|
}
|
|
|
|
if (!motorblimp) { //Finned blimp
|
|
//all fin setup
|
|
for (uint8_t i=0; i<4; i++) {
|
|
fin[i].last_angle = fin[i].angle;
|
|
if (!battery_is_empty()) {
|
|
if (input.servos[i] == 0) {
|
|
fin[i].angle = 0;
|
|
fin[i].servo_angle = 0;
|
|
} else {
|
|
// filtered_servo_angle() normalises PWM against a fixed 1500 +/- 500
|
|
// range, but blimp.parm uses SERVOn_MIN 500 / TRIM 1350 / MAX 2200;
|
|
// the +13.5 degree offset recentres that asymmetric range to a
|
|
// symmetric -76.5..+76.5 degrees with 0 degrees at servo trim
|
|
fin[i].angle = filtered_servo_angle(input, i)*radians(45.0f)+radians(13.5);
|
|
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 = 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[i].N; //normal force flips when fin changes direction
|
|
|
|
fin[i].Fx = 0;
|
|
fin[i].Fy = 0;
|
|
fin[i].Fz = 0;
|
|
}
|
|
|
|
//Back fin
|
|
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);// - 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);// - 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);// + 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 F_BF{0,0,0};
|
|
for (uint8_t i=0; i<4; i++) {
|
|
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;
|
|
}
|
|
|
|
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};
|
|
|
|
#if HAL_LOGGING_ENABLED
|
|
// @LoggerMessage: SFT
|
|
// @Description: Simulated Blimp Fin Thrust
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: f0: Fin 0 tangential thrust
|
|
// @Field: f1: Fin 1 tangential thrust
|
|
// @Field: f2: Fin 2 tangential thrust
|
|
// @Field: f3: Fin 3 tangential thrust
|
|
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);
|
|
// @LoggerMessage: SFN
|
|
// @Description: Simulated Blimp Fin Forces
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: n0: Fin 0 normal force
|
|
// @Field: n1: Fin 1 normal force
|
|
// @Field: n2: Fin 2 normal force
|
|
// @Field: n3: Fin 3 normal force
|
|
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);
|
|
// @LoggerMessage: SBA1
|
|
// @Description: Simulated Blimp Body-Frame accelerations
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: ax: x-axis acceleration
|
|
// @Field: ay: y-axis acceleration
|
|
// @Field: az: z-axis acceleration
|
|
AP::logger().WriteStreaming("SBA1", "TimeUS,ax,ay,az",
|
|
"Qfff",
|
|
AP_HAL::micros64(),
|
|
body_acc.x, body_acc.y, body_acc.z);
|
|
// @LoggerMessage: SFA1
|
|
// @Description: Simulated Blimp Fin Angles
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: f0: fin 0 angle
|
|
// @Field: f1: fin 1 angle
|
|
// @Field: f2: fin 2 angle
|
|
// @Field: f3: fin 3 angle
|
|
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);
|
|
// @LoggerMessage: SFAN
|
|
// @Description: Simulated Blimp Servo Angles
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: f0: fin 0 servo angle
|
|
// @Field: f1: fin 1 servo angle
|
|
// @Field: f2: fin 2 servo angle
|
|
// @Field: f3: fin 3 servo 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);
|
|
// @LoggerMessage: SSAN
|
|
// @Description: Simulated Blimp Servo Inputs
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: f0: fin 0 servo angle input
|
|
// @Field: f1: fin 1 servo angle input
|
|
// @Field: f2: fin 2 servo angle input
|
|
// @Field: f3: fin 3 servo angle input
|
|
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]);
|
|
// @LoggerMessage: SFV1
|
|
// @Description: Simulated Blimp Fin Velocities
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: f0: fin 0 velocity
|
|
// @Field: f1: fin 1 velocity
|
|
// @Field: f2: fin 2 velocity
|
|
// @Field: f3: fin 3 velocity
|
|
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);
|
|
// @LoggerMessage: SRT1
|
|
// @Description: Simulated Blimp Rotational forces
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: rtx: zero
|
|
// @Field: rty: zero
|
|
// @Field: rtz: zero
|
|
AP::logger().WriteStreaming("SRT1", "TimeUS,rtx,rty,rtz",
|
|
"Qfff",
|
|
AP_HAL::micros64(),
|
|
rot_T.x, rot_T.y, rot_T.z);
|
|
#endif // HAL_LOGGING_ENABLED
|
|
|
|
#if 0 //"Wobble" attempt
|
|
rot_T.y = fin[0].Fz * radius + fin[1].Fz * radius;
|
|
// @LoggerMessage: SRT2
|
|
// @Description: Transformed Simulated Blimp Rotational forces
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: rtx: x-axis wobble rotational force
|
|
// @Field: rty: y-axis wobble rotational force
|
|
// @Field: rtz: z-axis wobble rotational force
|
|
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;
|
|
// @LoggerMessage: SRT3
|
|
// @Description: Simulated Blimp Torques
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: rtx: torque around x axis
|
|
// @Field: rty: torque around y axis
|
|
// @Field: rtz: torque around z axis
|
|
AP::logger().WriteStreaming("SRT3", "TimeUS,rtx,rty,rtz",
|
|
"Qfff",
|
|
AP_HAL::micros64(),
|
|
rot_T.x, rot_T.y, rot_T.z);
|
|
// @LoggerMessage: SAN1
|
|
// @Description: Simulated Blimp Angles
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: anx: x angle
|
|
// @Field: any: y angle
|
|
// @Field: anz: z angle
|
|
AP::logger().WriteStreaming("SAN1", "TimeUS,anx,any,anz",
|
|
"Qfff",
|
|
AP_HAL::micros64(),
|
|
ang.x, ang.y, ang.z);
|
|
// @LoggerMessage: SAN2
|
|
// @Description: Simulated Blimp Angles
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: anx: x earth-frame angle
|
|
// @Field: any: y earth-frame angle
|
|
// @Field: anz: z earth-frame angle
|
|
AP::logger().WriteStreaming("SAN2", "TimeUS,anx,any,anz",
|
|
"Qfff",
|
|
AP_HAL::micros64(),
|
|
ang_ef.x, ang_ef.y, ang_ef.z);
|
|
// @LoggerMessage: SAF1
|
|
// @Description: Simulated Blimp Sin-Angles
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: afx: sin(x angle)
|
|
// @Field: afy: sin(y angle)
|
|
// @Field: afz: sin(z angle)
|
|
AP::logger().WriteStreaming("SAF1", "TimeUS,afx,afy,afz",
|
|
"Qfff",
|
|
AP_HAL::micros64(),
|
|
sinf(ang.x), sinf(ang.y), sinf(ang.z));
|
|
// @LoggerMessage: SMGC
|
|
// @Description: Simulated Blimp Mass and COG
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: m: mass
|
|
// @Field: g: gravity
|
|
// @Field: cz: centre-of-gravity, z-axis
|
|
AP::logger().WriteStreaming("SMGC", "TimeUS,m,g,cz",
|
|
"Qfff",
|
|
AP_HAL::micros64(),
|
|
mass, GRAVITY_MSS, cog.z);
|
|
#endif
|
|
|
|
//Blimp yaw
|
|
rot_T.z = fin[2].Fx * radius - fin[3].Fx * radius;//in N*m (Torque = force * lever arm)
|
|
//rot accel = torque / moment of inertia
|
|
//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;
|
|
|
|
} else { //MotorBlimp
|
|
for (uint8_t i=0; i<4; i++) {
|
|
if (battery_is_empty() || input.servos[i] == 0) {
|
|
mot[i].throttle = 0;
|
|
} else {
|
|
mot[i].throttle = filtered_servo_angle(input, i);
|
|
}
|
|
mot[i].thrust = mot[i].throttle * k_m;
|
|
|
|
mot[i].Fx = 0;
|
|
mot[i].Fy = 0;
|
|
mot[i].Fz = 0;
|
|
}
|
|
|
|
//FrontLeft motor
|
|
mot[0].Fx = mot[0].thrust;
|
|
//FrontRight motor
|
|
mot[1].Fx = mot[1].thrust;
|
|
//Up motor
|
|
mot[2].Fz = -mot[2].thrust;
|
|
//Right motor
|
|
mot[3].Fy = mot[3].thrust;
|
|
|
|
Vector3f F_BF{0,0,0};
|
|
for (uint8_t i=0; i<4; i++) {
|
|
F_BF.x = F_BF.x + mot[i].Fx;
|
|
F_BF.y = F_BF.y + mot[i].Fy;
|
|
F_BF.z = F_BF.z + mot[i].Fz;
|
|
}
|
|
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 = mot[0].thrust * gondolawidth - mot[1].thrust * gondolawidth; //in N*m (Torque = force * lever arm)
|
|
//skipping swing effect generated by the motors.
|
|
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;
|
|
}
|
|
}
|
|
|
|
/*
|
|
update the blimp simulation by one time step
|
|
*/
|
|
void Blimp::update(const struct sitl_input &input)
|
|
{
|
|
float delta_time = frame_time_us * 1.0e-6f;
|
|
|
|
Vector3f rot_accel = Vector3f(0,0,0);
|
|
calculate_forces(input, accel_body, rot_accel);
|
|
|
|
if (hal.scheduler->is_system_initialized()) {
|
|
// quadratic rotational drag, dominant at higher rates
|
|
float gyr_sq = gyro.length_squared();
|
|
if (is_positive(gyr_sq)) {
|
|
Vector3f force_gyr = (gyro.normalized() * drag_gyr_constant * gyr_sq);
|
|
Vector3f ef_drag_accel_gyr = -force_gyr / mass;
|
|
Vector3f bf_drag_accel_gyr = dcm.transposed() * ef_drag_accel_gyr;
|
|
rot_accel += bf_drag_accel_gyr;
|
|
}
|
|
// linear rotational drag so residual rates decay to zero
|
|
rot_accel -= gyro * (drag_gyr_lin_constant / mass);
|
|
}
|
|
|
|
// 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();
|
|
|
|
if (hal.scheduler->is_system_initialized()) {
|
|
// quadratic translational drag, dominant at higher speeds
|
|
float speed_sq = velocity_ef.length_squared();
|
|
if (is_positive(speed_sq)) {
|
|
Vector3f force = (velocity_ef.normalized() * drag_constant * speed_sq);
|
|
Vector3f ef_drag_accel = -force / mass;
|
|
Vector3f bf_drag_accel = dcm.transposed() * ef_drag_accel;
|
|
accel_body += bf_drag_accel;
|
|
}
|
|
// linear translational drag so residual velocity decays to zero
|
|
accel_body -= (dcm.transposed() * velocity_ef) * (drag_lin_constant / mass);
|
|
|
|
// 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;
|
|
position += (velocity_ef * delta_time).todouble(); //update position vector
|
|
|
|
update_position(); //updates the position from the Vector3f position
|
|
time_advance();
|
|
update_mag_field_bf();
|
|
rate_hz = sitl->loop_rate_hz;
|
|
|
|
update_battery();
|
|
}
|
|
|
|
void Blimp::update_battery()
|
|
{
|
|
battery.maybe_reset(sitl->batt_voltage, sitl->batt_capacity_ah);
|
|
battery_current = 0.0f;
|
|
|
|
if (!battery_is_empty()) {
|
|
if (motorblimp) {
|
|
constexpr float motor_current_scaler_amps = 2.0f;
|
|
for (uint8_t i=0; i<4; i++) {
|
|
battery_current += fabsf(mot[i].throttle) * motor_current_scaler_amps;
|
|
}
|
|
} else {
|
|
constexpr float fin_current_scaler_amps = 0.01f;
|
|
for (uint8_t i=0; i<4; i++) {
|
|
battery_current += fabsf(fin[i].vel) * fin_current_scaler_amps;
|
|
}
|
|
}
|
|
}
|
|
|
|
battery.consume_energy(battery_current, AP_HAL::micros64());
|
|
battery_voltage = battery.get_voltage();
|
|
battery_temperature_degC = battery.get_temperature_degC();
|
|
}
|