mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 07:28:29 -04:00
SITL: Don't start scrimmage from ArduPilot
This commit is contained in:
parent
e75ac4ae57
commit
98dfc69887
@ -36,44 +36,6 @@ Scrimmage::Scrimmage(const char *_frame_str) :
|
|||||||
send_sock(true),
|
send_sock(true),
|
||||||
frame_str(_frame_str)
|
frame_str(_frame_str)
|
||||||
{
|
{
|
||||||
// Set defaults for scrimmage-copter
|
|
||||||
if (strcmp(frame_str, "scrimmage-copter")== 0) {
|
|
||||||
mission_name = "arducopter.xml";
|
|
||||||
motion_model = "Multirotor";
|
|
||||||
visual_model = "iris";
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void Scrimmage::set_config(const char *config)
|
|
||||||
{
|
|
||||||
// The configuration string is a comma-separated sequence of key:value
|
|
||||||
// pairs
|
|
||||||
|
|
||||||
// Iterate over comma-separated strings and store parameters
|
|
||||||
char *end_str;
|
|
||||||
char* copy_config = strdup(config);
|
|
||||||
char *token = strtok_r(copy_config, ",", &end_str);
|
|
||||||
while (token != NULL)
|
|
||||||
{
|
|
||||||
char *end_token;
|
|
||||||
char *token2 = strtok_r(token, "=", &end_token);
|
|
||||||
if (strcmp(token2, "mission")==0) {
|
|
||||||
mission_name = strtok_r(NULL, "=", &end_token);
|
|
||||||
} else if (strcmp(token2, "motion_model")==0) {
|
|
||||||
motion_model = strtok_r(NULL, "=", &end_token);
|
|
||||||
} else if (strcmp(token2, "visual_model")==0) {
|
|
||||||
visual_model = strtok_r(NULL, "=", &end_token);
|
|
||||||
} else if (strcmp(token2, "terrain")==0) {
|
|
||||||
terrain = strtok_r(NULL, "=", &end_token);
|
|
||||||
} else {
|
|
||||||
printf("Invalid scrimmage param: %s", token2);
|
|
||||||
}
|
|
||||||
token = strtok_r(NULL, ",", &end_str);
|
|
||||||
}
|
|
||||||
free(copy_config);
|
|
||||||
|
|
||||||
// start scrimmage after parsing simulation configuration
|
|
||||||
start_scrimmage();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Scrimmage::set_interface_ports(const char* address, const int port_in, const int port_out)
|
void Scrimmage::set_interface_ports(const char* address, const int port_in, const int port_out)
|
||||||
@ -93,43 +55,6 @@ void Scrimmage::set_interface_ports(const char* address, const int port_in, cons
|
|||||||
send_sock.set_blocking(false);
|
send_sock.set_blocking(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Start Scrimmage child
|
|
||||||
void Scrimmage::start_scrimmage(void)
|
|
||||||
{
|
|
||||||
pid_t child_pid = fork();
|
|
||||||
if (child_pid == 0) {
|
|
||||||
// In child
|
|
||||||
|
|
||||||
// Construct the scrimmage command string with overrides for initial
|
|
||||||
// position and heading.
|
|
||||||
char* full_exec_str;
|
|
||||||
int len;
|
|
||||||
// Get required string length
|
|
||||||
len = snprintf(NULL, 0, "xterm +hold -T SCRIMMAGE -e 'scrimmage %s -o \"latitude_origin=%f,"
|
|
||||||
"longitude_origin=%f,altitude_origin=%f,heading=%f,motion_model=%s,visual_model=%s,terrain=%s,"
|
|
||||||
"to_ardupilot_port=%d,from_ardupilot_port=%d,to_ardupilot_ip=%s\"'", mission_name, home.lat*1.0e-7f,home.lng*1.0e-7f,
|
|
||||||
home.alt*1.0e-2f, home_yaw, motion_model, visual_model, terrain, fdm_port_in, fdm_port_out, fdm_address);
|
|
||||||
|
|
||||||
full_exec_str = (char *)malloc(len+1);
|
|
||||||
snprintf(full_exec_str, len+1, "xterm +hold -T SCRIMMAGE -e 'scrimmage %s -o \"latitude_origin=%f,"
|
|
||||||
"longitude_origin=%f,altitude_origin=%f,heading=%f,motion_model=%s,visual_model=%s,terrain=%s,"
|
|
||||||
"to_ardupilot_port=%d,from_ardupilot_port=%d,to_ardupilot_ip=%s\"'", mission_name, home.lat*1.0e-7f,home.lng*1.0e-7f,
|
|
||||||
home.alt*1.0e-2f, home_yaw, motion_model, visual_model, terrain, fdm_port_in, fdm_port_out, fdm_address);
|
|
||||||
|
|
||||||
printf("%s\n", full_exec_str);
|
|
||||||
|
|
||||||
// system call worked
|
|
||||||
int ret = system(full_exec_str);
|
|
||||||
|
|
||||||
if (ret != 0) {
|
|
||||||
::fprintf(stderr, "scrimmage didn't open.\n");
|
|
||||||
perror("scrimmage");
|
|
||||||
}
|
|
||||||
|
|
||||||
free(full_exec_str);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void Scrimmage::send_servos(const struct sitl_input &input)
|
void Scrimmage::send_servos(const struct sitl_input &input)
|
||||||
{
|
{
|
||||||
servo_packet pkt;
|
servo_packet pkt;
|
||||||
|
@ -41,8 +41,6 @@ public:
|
|||||||
return new Scrimmage(frame_str);
|
return new Scrimmage(frame_str);
|
||||||
}
|
}
|
||||||
|
|
||||||
void set_config(const char *config) override;
|
|
||||||
|
|
||||||
/* Create and set in/out socket for extenal simulator */
|
/* Create and set in/out socket for extenal simulator */
|
||||||
void set_interface_ports(const char* address, const int port_in, const int port_out) override;
|
void set_interface_ports(const char* address, const int port_in, const int port_out) override;
|
||||||
|
|
||||||
@ -76,19 +74,12 @@ private:
|
|||||||
|
|
||||||
void recv_fdm(const struct sitl_input &input);
|
void recv_fdm(const struct sitl_input &input);
|
||||||
void send_servos(const struct sitl_input &input);
|
void send_servos(const struct sitl_input &input);
|
||||||
void start_scrimmage(void);
|
|
||||||
|
|
||||||
uint64_t prev_timestamp_us;
|
uint64_t prev_timestamp_us;
|
||||||
SocketAPM recv_sock;
|
SocketAPM recv_sock;
|
||||||
SocketAPM send_sock;
|
SocketAPM send_sock;
|
||||||
|
|
||||||
const char *frame_str;
|
const char *frame_str;
|
||||||
|
|
||||||
// Use ArduPlane by default
|
|
||||||
const char *mission_name = "arduplane.xml";
|
|
||||||
const char *motion_model = "JSBSimControl";
|
|
||||||
const char *visual_model = "zephyr-blue";
|
|
||||||
const char *terrain = "mcmillan";
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace SITL
|
} // namespace SITL
|
||||||
|
Loading…
Reference in New Issue
Block a user