ardupilot/libraries/SITL/SIM_CRRCSim.cpp

157 lines
4.4 KiB
C++

/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
simulator connector for ardupilot version of CRRCSim
*/
#include "SIM_CRRCSim.h"
#include <stdio.h>
#include <AP_HAL/AP_HAL.h>
extern const AP_HAL::HAL& hal;
namespace SITL {
CRRCSim::CRRCSim(const char *frame_str) :
Aircraft(frame_str),
last_timestamp(0),
sock(true)
{
// try to bind to a specific port so that if we restart ArduPilot
// CRRCSim keeps sending us packets. Not strictly necessary but
// useful for debugging
sock.bind("127.0.0.1", 9003);
sock.reuseaddress();
sock.set_blocking(false);
heli_servos = (strstr(frame_str,"heli") != nullptr);
}
/*
decode and send servos for heli
*/
void CRRCSim::send_servos_heli(const struct sitl_input &input)
{
float swash1 = (input.servos[0]-1000) / 1000.0f;
float swash2 = (input.servos[1]-1000) / 1000.0f;
float swash3 = (input.servos[2]-1000) / 1000.0f;
float tail_rotor = (input.servos[3]-1000) / 1000.0f;
float rsc = (input.servos[7]-1000) / 1000.0f;
float col_pitch = (swash1+swash2+swash3)/3.0 - 0.5f;
float roll_rate = (swash1 - swash2)/2;
float pitch_rate = -((swash1 + swash2)/2.0 - swash3)/2;
float yaw_rate = -(tail_rotor - 0.5);
servo_packet pkt;
pkt.roll_rate = constrain_float(roll_rate, -0.5, 0.5);
pkt.pitch_rate = constrain_float(pitch_rate, -0.5, 0.5);
pkt.throttle = constrain_float(rsc, 0, 1);
pkt.yaw_rate = constrain_float(yaw_rate, -0.5, 0.5);
pkt.col_pitch = constrain_float(col_pitch, -0.5, 0.5);
sock.sendto(&pkt, sizeof(pkt), "127.0.0.1", 9002);
}
/*
decode and send servos for fixed wing
*/
void CRRCSim::send_servos_fixed_wing(const struct sitl_input &input)
{
float roll_rate = ((input.servos[0]-1000)/1000.0) - 0.5;
float pitch_rate = ((input.servos[1]-1000)/1000.0) - 0.5;
float yaw_rate = ((input.servos[3]-1000)/1000.0) - 0.5;
float throttle = ((input.servos[2]-1000)/1000.0);
servo_packet pkt;
pkt.roll_rate = constrain_float(roll_rate, -0.5, 0.5);
pkt.pitch_rate = constrain_float(pitch_rate, -0.5, 0.5);
pkt.throttle = constrain_float(throttle, 0, 1);
pkt.yaw_rate = constrain_float(yaw_rate, -0.5, 0.5);
pkt.col_pitch = 0;
sock.sendto(&pkt, sizeof(pkt), "127.0.0.1", 9002);
}
/*
decode and send servos
*/
void CRRCSim::send_servos(const struct sitl_input &input)
{
if (heli_servos) {
send_servos_heli(input);
} else {
send_servos_fixed_wing(input);
}
}
/*
receive an update from the FDM
This is a blocking function
*/
void CRRCSim::recv_fdm(const struct sitl_input &input)
{
fdm_packet pkt;
/*
we re-send the servo packet every 0.1 seconds until we get a
reply. This allows us to cope with some packet loss to the FDM
*/
while (sock.recv(&pkt, sizeof(pkt), 100) != sizeof(pkt)) {
send_servos(input);
}
accel_body = Vector3f(pkt.xAccel, pkt.yAccel, pkt.zAccel);
gyro = Vector3f(pkt.rollRate, pkt.pitchRate, pkt.yawRate);
velocity_ef = Vector3f(pkt.speedN, pkt.speedE, pkt.speedD);
origin.lat = pkt.latitude * 1.0e7;
origin.lng = pkt.longitude * 1.0e7;
position.xy().zero();
position.z = -pkt.altitude;
airspeed = pkt.airspeed;
airspeed_pitot = pkt.airspeed;
dcm.from_euler(pkt.roll, pkt.pitch, pkt.yaw);
// auto-adjust to crrcsim frame rate
double deltat = pkt.timestamp - last_timestamp;
time_now_us += deltat * 1.0e6;
if (deltat < 0.01 && deltat > 0) {
adjust_frame_time(1.0/deltat);
}
last_timestamp = pkt.timestamp;
}
/*
update the CRRCSim simulation by one time step
*/
void CRRCSim::update(const struct sitl_input &input)
{
send_servos(input);
recv_fdm(input);
update_position();
time_advance();
// update magnetic field
update_mag_field_bf();
}
} // namespace SITL