mirror of https://github.com/ArduPilot/ardupilot
121 lines
3.9 KiB
C++
121 lines
3.9 KiB
C++
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#include <SITL/SIM_Motor.h>
|
|
#include <SITL/SIM_Frame.h>
|
|
#include <SITL/SITL_Input.h>
|
|
|
|
#include <stdio.h>
|
|
|
|
/* run with:
|
|
./waf configure --board linux
|
|
./waf build --targets examples/EvaluateMotorModel
|
|
./build/linux/examples/EvaluateMotorModel Tools/autotest/models/Callisto.json 0 50
|
|
*/
|
|
|
|
void setup();
|
|
void loop();
|
|
|
|
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
|
|
|
// helper to give access to protected functions
|
|
class Frame_helper : public SITL::Frame {
|
|
public:
|
|
using SITL::Frame::Frame;
|
|
|
|
void public_load_frame_params(const char *model_json) {
|
|
load_frame_params(model_json);
|
|
}
|
|
|
|
float public_get_air_density(float alt_amsl) {
|
|
return get_air_density(alt_amsl);
|
|
}
|
|
|
|
float get_pwm_min() { return model.pwmMin; }
|
|
float get_pwm_max() { return model.pwmMax; }
|
|
float get_spin_min() { return model.spin_min; }
|
|
float get_spin_max() { return model.spin_max; }
|
|
float get_prop_expo() { return model.propExpo; }
|
|
float get_slew_max() { return model.slew_max; }
|
|
float get_max_voltage() { return model.maxVoltage; }
|
|
float get_mass() { return model.mass; }
|
|
float get_ref_current() { return model.refCurrent; }
|
|
float get_ref_voltage() { return model.refVoltage; }
|
|
float get_ref_alt() { return model.refAlt; }
|
|
float get_hover_thr_out() { return model.hoverThrOut; }
|
|
float get_disk_area() { return model.disc_area; }
|
|
float get_mdrag_coef() { return model.mdrag_coef; }
|
|
float get_num_motors() { return model.num_motors; }
|
|
|
|
};
|
|
|
|
Frame_helper frame {nullptr, 0, nullptr};
|
|
SITL::Motor motor{0,0,1,1};
|
|
|
|
void setup(void)
|
|
{
|
|
uint8_t argc;
|
|
char * const *argv;
|
|
|
|
hal.util->commandline_arguments(argc, argv);
|
|
|
|
if (argc <= 2) {
|
|
::printf("pass .json model definition path and velocity\n");
|
|
exit(0);
|
|
}
|
|
|
|
// parse JSON vehicle definition
|
|
frame.public_load_frame_params(argv[1]);
|
|
|
|
// duplicate of the calculations in SIM_Frame.cpp
|
|
float ref_air_density = frame.public_get_air_density(frame.get_ref_alt());
|
|
float hover_thrust = frame.get_mass() * GRAVITY_MSS;
|
|
float hover_power = frame.get_ref_current() * frame.get_ref_voltage();
|
|
float hover_velocity_out = 2 * hover_power / hover_thrust;
|
|
float effective_disc_area = hover_thrust / (0.5 * ref_air_density * sq(hover_velocity_out));
|
|
float velocity_max = hover_velocity_out / sqrtf(frame.get_hover_thr_out());
|
|
float power_factor = hover_power / hover_thrust;
|
|
float effective_prop_area = effective_disc_area / frame.get_num_motors();
|
|
float true_prop_area = frame.get_disk_area() / frame.get_num_motors();
|
|
|
|
motor.setup_params(frame.get_pwm_min(), frame.get_pwm_max(), frame.get_spin_min(), frame.get_spin_max(), frame.get_prop_expo(), frame.get_slew_max(),
|
|
0, power_factor, frame.get_max_voltage(), effective_prop_area, velocity_max,
|
|
Vector3f {}, Vector3f {}, 1, true_prop_area, frame.get_mdrag_coef());
|
|
|
|
// parse inflow velocity and voltage
|
|
const float velocity = strtof(argv[2], NULL);
|
|
Vector3f velocity3 {0,0,-velocity};
|
|
|
|
const float voltage = strtof(argv[3], NULL);
|
|
|
|
::printf("Motors at %0.2fv with %0.2f m/s inflow\n", voltage, velocity);
|
|
|
|
uint64_t time = 0;
|
|
hal.scheduler->stop_clock(time);
|
|
const double time_step = 0.05;
|
|
|
|
struct sitl_input input;
|
|
|
|
|
|
::printf("time, PWM, thrust, torque, current\n");
|
|
const Vector3f gyro {};
|
|
for (uint16_t PWM = frame.get_pwm_min(); PWM <= frame.get_pwm_max(); PWM++) {
|
|
input.servos[0] = PWM;
|
|
|
|
Vector3f torque;
|
|
Vector3f thrust;
|
|
motor.calculate_forces(input, 0, torque, thrust, velocity3, gyro, ref_air_density, voltage, true);
|
|
|
|
::printf("%0.2f, %u, %0.2f, %0.2f, %0.2f\n", time * 1.0e-6, PWM, -thrust.z, torque.z, motor.get_current());
|
|
|
|
time += time_step*1e6;
|
|
hal.scheduler->stop_clock(time);
|
|
}
|
|
}
|
|
|
|
void loop(void)
|
|
{
|
|
exit(0);
|
|
}
|
|
|
|
AP_HAL_MAIN();
|