mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-06 19:00:27 +08:00
Andrew Tridgell
fe8c06f869
allows the scalar model values (mass, disc area, momentum drag etc) to be customised as parameters for copters and quadplanes. A json model file sets the defaults for the parameters The SIM_FRM_BBDRAG and SIM_FRM_MDRAG drag coefficients default to zero on planes so the plane model alone handles drag and the auto-tests don't need retuning. Setting them on a quadplane gives more realistic airbraking in VTOL modes
108 lines
3.3 KiB
C++
108 lines
3.3 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/>.
|
|
*/
|
|
/*
|
|
multicopter simulator class
|
|
*/
|
|
|
|
#include "SIM_Multicopter.h"
|
|
#include <AP_Motors/AP_Motors.h>
|
|
|
|
#include <stdio.h>
|
|
|
|
using namespace SITL;
|
|
|
|
MultiCopter::MultiCopter(const char *frame_str) :
|
|
Aircraft(frame_str)
|
|
{
|
|
frame = Frame::create_frame(frame_str);
|
|
if (frame == nullptr) {
|
|
printf("Frame '%s' not found or insufficient memory", frame_str);
|
|
exit(1);
|
|
}
|
|
|
|
frame->init(frame_str);
|
|
battery.setup(frame->get_model_batt_capacity_ah(),
|
|
frame->get_model_batt_resistance_ohm(),
|
|
frame->get_model_batt_max_voltage(),
|
|
ambient_outside_temperature_degC());
|
|
|
|
mass = frame->get_mass();
|
|
frame_height = 0.1;
|
|
ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
|
|
lock_step_scheduled = true;
|
|
}
|
|
|
|
// calculate rotational and linear accelerations
|
|
void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel)
|
|
{
|
|
motor_mask |= ((1U<<frame->num_motors)-1U) << frame->motor_offset;
|
|
frame->calculate_forces(*this, input, rot_accel, body_accel, rpm);
|
|
|
|
add_shove_forces(rot_accel, body_accel);
|
|
add_twist_forces(rot_accel);
|
|
|
|
// add forces from slung payload or tether payload
|
|
add_external_forces(body_accel);
|
|
}
|
|
|
|
/*
|
|
update the multicopter simulation by one time step
|
|
*/
|
|
void MultiCopter::update(const struct sitl_input &input)
|
|
{
|
|
// refresh mass in case SIM_FRM_ parameters have changed
|
|
mass = frame->get_mass();
|
|
|
|
// get wind vector setup
|
|
update_wind(input);
|
|
|
|
Vector3f rot_accel;
|
|
|
|
calculate_forces(input, rot_accel, accel_body);
|
|
// simulated clamp holding vehicle down
|
|
if (clamp.clamped(*this, input)) {
|
|
rot_accel.zero();
|
|
accel_body.zero();
|
|
}
|
|
|
|
update_battery();
|
|
update_dynamics(rot_accel);
|
|
update_external_payload(input);
|
|
|
|
// update lat/lon/altitude
|
|
update_position();
|
|
time_advance();
|
|
|
|
// update magnetic field
|
|
update_mag_field_bf();
|
|
}
|
|
|
|
void MultiCopter::update_battery() {
|
|
if (frame->battery_changed()) {
|
|
// battery model changed via SIM_FRM_ parameters
|
|
battery.setup(frame->get_model_batt_capacity_ah(),
|
|
frame->get_model_batt_resistance_ohm(),
|
|
frame->get_model_batt_max_voltage(),
|
|
ambient_outside_temperature_degC());
|
|
}
|
|
battery.maybe_reset(sitl->batt_voltage, sitl->batt_capacity_ah, sitl->batt_resistance);
|
|
battery_voltage = battery.get_voltage();
|
|
battery_current = frame->get_current_amp();
|
|
battery_temperature_degC = battery.get_temperature_degC();
|
|
|
|
const uint64_t now_us = AP_HAL::micros64();
|
|
battery.consume_energy(battery_current, now_us);
|
|
}
|