Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-12 01:23:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

SITL: Add rover support for Airsim

Browse Source
This commit is contained in:
Rajat Singhal 2019-07-31 12:43:40 +05:30 committed by Peter Barker
parent 842822bb47
commit 6bbf6f380f
2 changed files with 57 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
libraries/SITL/SIM_AirSim.cpp
View File

@ -34,7 +34,16 @@ AirSim::AirSim(const char *frame_str) :
Aircraft(frame_str),
sock(true)
{
printf("Starting SITL Airsim\n");
if (strstr(frame_str, "-copter")) {
output_type = OutputType::Copter;
} else if (strstr(frame_str, "-rover")) {
output_type = OutputType::Rover;
} else {
// default to copter
output_type = OutputType::Copter;
}
printf("Starting SITL Airsim type %u\n", (unsigned)output_type);
}
/*
@ -61,9 +70,9 @@ void AirSim::set_interface_ports(const char* address, const int port_in, const i
/*
Decode and send servos
*/
void AirSim::send_servos(const struct sitl_input &input)
void AirSim::output_copter(const struct sitl_input &input)
{
servo_packet pkt{0};
servo_packet pkt;
for (uint8_t i=0; i<kArduCopterRotorControlCount; i++) {
pkt.pwm[i] = input.servos[i];
@ -80,6 +89,24 @@ void AirSim::send_servos(const struct sitl_input &input)
}
}
void AirSim::output_rover(const struct sitl_input &input)
{
rover_packet pkt;
pkt.steering = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
pkt.throttle = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
ssize_t send_ret = sock.sendto(&pkt, sizeof(pkt), airsim_ip, airsim_control_port);
if (send_ret != sizeof(pkt)) {
if (send_ret <= 0) {
printf("Unable to send control output to %s:%u - Error: %s, Return value: %ld\n",
airsim_ip, airsim_control_port, strerror(errno), (long)send_ret);
} else {
printf("Sent %ld bytes instead of %lu bytes\n", (long)send_ret, (unsigned long)sizeof(pkt));
}
}
}
/*
very simple JSON parser for sensor data
called with pointer to one row of sensor data, nul terminated
@ -320,7 +347,16 @@ void AirSim::recv_fdm()
*/
void AirSim::update(const struct sitl_input &input)
{
send_servos(input);
switch (output_type) {
case OutputType::Copter:
output_copter(input);
break;
case OutputType::Rover:
output_rover(input);
break;
}
recv_fdm();
// update magnetic field

24
libraries/SITL/SIM_AirSim.h
View File

@ -43,12 +43,21 @@ public:
void set_interface_ports(const char* address, const int port_in, const int port_out) override;
private:
/*
rotor control packet sent by Ardupilot
*/
static const int kArduCopterRotorControlCount = 11;
enum class OutputType {
Copter = 1,
Rover = 2
} output_type;
struct servo_packet {
// Control packet for Rover
struct rover_packet {
float throttle; // -1 to 1
float steering; // -1 to 1
};
// rotor control packet sent by Ardupilot
static const int kArduCopterRotorControlCount = 11;
struct servo_packet {
uint16_t pwm[kArduCopterRotorControlCount];
};
@ -68,8 +77,9 @@ private:
uint64_t last_frame_count;
uint64_t last_timestamp;
void send_servos(const struct sitl_input &input);
void recv_fdm();
void output_copter(const struct sitl_input &input);
void output_rover(const struct sitl_input &input);
void recv_fdm();
void report_FPS(void);
bool parse_sensors(const char *json);