/* 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 . */ /* simulator connector for ardupilot version of CRRCSim */ #include "SIM_CRRCSim.h" #if HAL_SIM_CRRCSIM_ENABLED #include #include 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 #endif // HAL_SIM_CRRCSIM_ENABLED