mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 15:08:28 -04:00
767773da5e
this takes account of motor expo, velocity of air over propellers, mass, size and other factors It also allows for frame parameters to be supplied as an external json file
139 lines
4.1 KiB
C++
139 lines
4.1 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/>.
|
|
*/
|
|
/*
|
|
simple quadplane simulator class
|
|
*/
|
|
|
|
#include "SIM_QuadPlane.h"
|
|
|
|
#include <stdio.h>
|
|
|
|
using namespace SITL;
|
|
|
|
QuadPlane::QuadPlane(const char *frame_str) :
|
|
Plane(frame_str)
|
|
{
|
|
// default to X frame
|
|
const char *frame_type = "x";
|
|
uint8_t motor_offset = 4;
|
|
|
|
if (strstr(frame_str, "-octa-quad")) {
|
|
frame_type = "octa-quad";
|
|
} else if (strstr(frame_str, "-octaquad")) {
|
|
frame_type = "octa-quad";
|
|
} else if (strstr(frame_str, "-octa")) {
|
|
frame_type = "octa";
|
|
} else if (strstr(frame_str, "-hexax")) {
|
|
frame_type = "hexax";
|
|
} else if (strstr(frame_str, "-hexa")) {
|
|
frame_type = "hexa";
|
|
} else if (strstr(frame_str, "-plus")) {
|
|
frame_type = "+";
|
|
} else if (strstr(frame_str, "-y6")) {
|
|
frame_type = "y6";
|
|
} else if (strstr(frame_str, "-tri")) {
|
|
frame_type = "tri";
|
|
} else if (strstr(frame_str, "-tilttrivec")) {
|
|
frame_type = "tilttrivec";
|
|
// fwd motor gives zero thrust
|
|
thrust_scale = 0;
|
|
} else if (strstr(frame_str, "-tilthvec")) {
|
|
frame_type = "tilthvec";
|
|
} else if (strstr(frame_str, "-tilttri")) {
|
|
frame_type = "tilttri";
|
|
// fwd motor gives zero thrust
|
|
thrust_scale = 0;
|
|
} else if (strstr(frame_str, "firefly")) {
|
|
frame_type = "firefly";
|
|
// elevon style surfaces
|
|
elevons = true;
|
|
// fwd motor gives zero thrust
|
|
thrust_scale = 0;
|
|
// vtol motors start at 2
|
|
motor_offset = 2;
|
|
} else if (strstr(frame_str, "cl84")) {
|
|
frame_type = "tilttri";
|
|
// fwd motor gives zero thrust
|
|
thrust_scale = 0;
|
|
}
|
|
frame = Frame::find_frame(frame_type);
|
|
if (frame == nullptr) {
|
|
printf("Failed to find frame '%s'\n", frame_type);
|
|
exit(1);
|
|
}
|
|
num_motors = 1 + frame->num_motors;
|
|
|
|
if (strstr(frame_str, "cl84")) {
|
|
// setup retract servos at front
|
|
frame->motors[0].servo_type = Motor::SERVO_RETRACT;
|
|
frame->motors[0].servo_rate = 7*60.0/90; // 7 seconds to change
|
|
frame->motors[1].servo_type = Motor::SERVO_RETRACT;
|
|
frame->motors[1].servo_rate = 7*60.0/90; // 7 seconds to change
|
|
}
|
|
|
|
// leave first 4 servos free for plane
|
|
frame->motor_offset = motor_offset;
|
|
|
|
// we use zero terminal velocity to let the plane model handle the drag
|
|
frame->init(frame_str);
|
|
|
|
ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
|
|
}
|
|
|
|
/*
|
|
update the quadplane simulation by one time step
|
|
*/
|
|
void QuadPlane::update(const struct sitl_input &input)
|
|
{
|
|
// get wind vector setup
|
|
update_wind(input);
|
|
|
|
// first plane forces
|
|
Vector3f rot_accel;
|
|
calculate_forces(input, rot_accel, accel_body);
|
|
|
|
// now quad forces
|
|
Vector3f quad_rot_accel;
|
|
Vector3f quad_accel_body;
|
|
|
|
frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body, &rpm[1], false);
|
|
|
|
// estimate voltage and current
|
|
frame->current_and_voltage(battery_voltage, battery_current);
|
|
|
|
float throttle;
|
|
if (reverse_thrust) {
|
|
throttle = filtered_servo_angle(input, 2);
|
|
} else {
|
|
throttle = filtered_servo_range(input, 2);
|
|
}
|
|
// assume 20A at full fwd throttle
|
|
throttle = fabsf(throttle);
|
|
battery_current += 20 * throttle;
|
|
|
|
rot_accel += quad_rot_accel;
|
|
accel_body += quad_accel_body;
|
|
|
|
update_dynamics(rot_accel);
|
|
update_external_payload(input);
|
|
|
|
// update lat/lon/altitude
|
|
update_position();
|
|
time_advance();
|
|
|
|
// update magnetic field
|
|
update_mag_field_bf();
|
|
}
|