aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: allow quadplane simulator to simulate other frame types

Browse Source
This commit is contained in:
Andrew Tridgell 2016-02-02 11:34:29 +11:00
parent 8655fdfd7f
commit 5efe98a814
3 changed files with 46 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/SITL/SIM_Multicopter.cpp
View File

@ -112,15 +112,24 @@ void Frame::init(float _mass, float hover_throttle, float _terminal_velocity, fl
terminal_rotation_rate = _terminal_rotation_rate;
}
/*
find a frame by name
*/
Frame *Frame::find_frame(const char *name)
{
for (uint8_t i=0; i < ARRAY_SIZE(supported_frames); i++) {
if (strcasecmp(name, supported_frames[i].name) == 0) {
return &supported_frames[i];
}
}
return NULL;
}
MultiCopter::MultiCopter(const char *home_str, const char *frame_str) :
Aircraft(home_str, frame_str),
frame(NULL)
{
for (uint8_t i=0; i < ARRAY_SIZE(supported_frames); i++) {
if (strcasecmp(frame_str, supported_frames[i].name) == 0) {
frame = &supported_frames[i];
}
}
frame = Frame::find_frame(frame_str);
if (frame == NULL) {
printf("Frame '%s' not found", frame_str);
exit(1);
@ -138,7 +147,7 @@ void Frame::calculate_forces(const Aircraft &aircraft,
float motor_speed[num_motors];
for (uint8_t i=0; i<num_motors; i++) {
uint16_t servo = input.servos[motors[i].servo-1];
uint16_t servo = input.servos[motor_offset+motors[i].servo-1];
// assume 1000 to 2000 PWM range
if (servo <= 1000) {
motor_speed[i] = 0;

5
libraries/SITL/SIM_Multicopter.h
View File

@ -55,6 +55,10 @@ public:
num_motors(_num_motors),
motors(_motors) {}
// find a frame by name
static Frame *find_frame(const char *name);
// initialise frame
void init(float mass, float hover_throttle, float terminal_velocity, float terminal_rotation_rate);
@ -67,6 +71,7 @@ public:
float terminal_rotation_rate;
float thrust_scale;
float mass;
uint8_t motor_offset;
};
/*

40
libraries/SITL/SIM_QuadPlane.cpp
View File

@ -23,27 +23,39 @@
using namespace SITL;
static const Motor quad_motors[4] =
{
Motor(45, false, 5),
Motor(225, false, 6),
Motor(315, true, 7),
Motor(135, true, 8)
};
static Frame quad_frame("x", 4, quad_motors);
QuadPlane::QuadPlane(const char *home_str, const char *frame_str) :
Plane(home_str, frame_str)
{
frame = &quad_frame;
// we use zero terminal velocity to let the plane model handle the drag
frame->init(mass, 0.51, 0, 0);
// see if we want a tiltrotor
if (strstr(frame_str, "-tilt")) {
tiltrotors = true;
}
// default to X frame
const char *frame_type = "x";
if (strstr(frame_str, "-octa-quad")) {
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 = "+";
}
frame = Frame::find_frame(frame_type);
if (frame == nullptr) {
printf("Failed to find frame '%s'\n", frame_type);
exit(1);
}
// leave first 4 servos free for plane
frame->motor_offset = 4;
// we use zero terminal velocity to let the plane model handle the drag
frame->init(mass, 0.51, 0, 0);
}
/*