2015-05-05 08:28:38 -03:00
|
|
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
/*
|
|
|
|
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
|
|
|
|
*/
|
|
|
|
|
2015-08-11 03:28:46 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2015-05-05 08:28:38 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
|
|
#include "SIM_CRRCSim.h"
|
|
|
|
#include <stdio.h>
|
|
|
|
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
|
|
|
/*
|
|
|
|
constructor
|
|
|
|
*/
|
|
|
|
CRRCSim::CRRCSim(const char *home_str, const char *frame_str) :
|
|
|
|
Aircraft(home_str, 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") != NULL);
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
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);
|
|
|
|
|
2015-05-22 03:05:48 -03:00
|
|
|
sock.sendto(&pkt, sizeof(pkt), "127.0.0.1", 9002);
|
2015-05-05 08:28:38 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
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;
|
|
|
|
|
2015-05-22 03:05:48 -03:00
|
|
|
sock.sendto(&pkt, sizeof(pkt), "127.0.0.1", 9002);
|
2015-05-05 08:28:38 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
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
|
|
|
|
*/
|
2015-05-22 02:52:58 -03:00
|
|
|
while (sock.recv(&pkt, sizeof(pkt), 100) != sizeof(pkt)) {
|
2015-05-05 08:28:38 -03:00
|
|
|
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);
|
|
|
|
|
|
|
|
Location loc1, loc2;
|
|
|
|
loc2.lat = pkt.latitude * 1.0e7;
|
|
|
|
loc2.lng = pkt.longitude * 1.0e7;
|
|
|
|
memset(&loc1, 0, sizeof(loc1));
|
|
|
|
Vector2f posdelta = location_diff(loc1, loc2);
|
|
|
|
position.x = posdelta.x;
|
|
|
|
position.y = posdelta.y;
|
|
|
|
position.z = -pkt.altitude;
|
|
|
|
|
2015-05-22 22:35:56 -03:00
|
|
|
airspeed = pkt.airspeed;
|
2015-05-22 22:24:10 -03:00
|
|
|
|
2015-05-05 08:28:38 -03:00
|
|
|
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();
|
|
|
|
}
|
|
|
|
#endif // CONFIG_HAL_BOARD
|