Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 08:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: allow override of Morse sensors and control ports

Browse Source
This commit is contained in:
Andrew Tridgell 2018-12-06 18:11:43 +11:00
parent 234e3b97ef
commit 824d619ab6
2 changed files with 49 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
libraries/SITL/SIM_Morse.cpp
View File

@ -83,15 +83,33 @@ static const struct {
Morse::Morse(const char *home_str, const char *frame_str) :
Aircraft(home_str, frame_str)
{
const char *colon = strchr(frame_str, ':');
if (colon) {
morse_ip = colon+1;
char *saveptr = nullptr;
char *s = strdup(frame_str);
char *frame_option = strtok_r(s, ":", &saveptr);
char *args1 = strtok_r(nullptr, ":", &saveptr);
char *args2 = strtok_r(nullptr, ":", &saveptr);
char *args3 = strtok_r(nullptr, ":", &saveptr);
/*
allow setting of IP, sensors port and control port
format morse:IPADDRESS:SENSORS_PORT:CONTROL_PORT
*/
if (args1) {
morse_ip = args1;
}
if (args2) {
morse_sensors_port = atoi(args2);
morse_control_port = morse_sensors_port+1;
}
if (args3) {
morse_control_port = atoi(args3);
}
if (strstr(frame_str, "-rover")) {
if (strstr(frame_option, "-rover")) {
output_type = OUTPUT_ROVER;
} else if (strstr(frame_str, "-quad")) {
} else if (strstr(frame_option, "-quad")) {
output_type = OUTPUT_QUAD;
} else if (strstr(frame_option, "-pwm")) {
output_type = OUTPUT_PWM;
} else {
// default to rover
output_type = OUTPUT_ROVER;
@ -107,7 +125,9 @@ Morse::Morse(const char *home_str, const char *frame_str) :
}
}
}
printf("Started Morse backend\n");
printf("Started Morse with %s:%u:%u type %u\n",
morse_ip, morse_sensors_port, morse_control_port,
(unsigned)output_type);
}
/*
@ -369,6 +389,22 @@ void Morse::output_quad(const struct sitl_input &input)
control_sock->send(buf, strlen(buf));
}
/*
output all 16 channels as PWM values. This allows for general
control of a robot
*/
void Morse::output_pwm(const struct sitl_input &input)
{
char buf[200];
snprintf(buf, sizeof(buf)-1, "{\"pwm\": [%u, %uf, %u, %u, %u, %uf, %u, %u, %u, %uf, %u, %u, %u, %uf, %u, %u]}\n",
input.servos[0], input.servos[1], input.servos[2], input.servos[3],
input.servos[4], input.servos[5], input.servos[6], input.servos[7],
input.servos[8], input.servos[9], input.servos[10], input.servos[11],
input.servos[12], input.servos[13], input.servos[14], input.servos[15]);
buf[sizeof(buf)-1] = 0;
control_sock->send(buf, strlen(buf));
}
/*
update the Morse simulation by one time step
@ -485,6 +521,9 @@ void Morse::update(const struct sitl_input &input)
case OUTPUT_QUAD:
output_quad(input);
break;
case OUTPUT_PWM:
output_pwm(input);
break;
}
report_FPS();

6
libraries/SITL/SIM_Morse.h
View File

@ -48,8 +48,9 @@ private:
uint16_t morse_control_port = 60001;
enum {
OUTPUT_ROVER,
OUTPUT_QUAD
OUTPUT_ROVER=1,
OUTPUT_QUAD=2,
OUTPUT_PWM=3
} output_type;
bool connect_sockets(void);
@ -57,6 +58,7 @@ private:
bool sensors_receive(void);
void output_rover(const struct sitl_input &input);
void output_quad(const struct sitl_input &input);
void output_pwm(const struct sitl_input &input);
void report_FPS();
// buffer for parsing pose data in JSON format