SITL: remove trailing whitespace

This commit is contained in:
Caio Marcelo de Oliveira Filho 2015-10-22 11:15:20 -02:00
parent 49a42dc985
commit f0a20cdc7c
12 changed files with 46 additions and 46 deletions

View File

@ -152,9 +152,9 @@ void Aircraft::adjust_frame_time(float new_rate)
} }
} }
/* /*
try to synchronise simulation time with wall clock time, taking try to synchronise simulation time with wall clock time, taking
into account desired speedup into account desired speedup
This tries to take account of possible granularity of This tries to take account of possible granularity of
get_wall_time_us() so it works reasonably well on windows get_wall_time_us() so it works reasonably well on windows
*/ */
@ -173,7 +173,7 @@ void Aircraft::sync_frame_time(void)
} }
#if 0 #if 0
::printf("achieved_rate_hz=%.3f rate=%.2f rate_hz=%.3f sft=%.1f\n", ::printf("achieved_rate_hz=%.3f rate=%.2f rate_hz=%.3f sft=%.1f\n",
(double)achieved_rate_hz, (double)achieved_rate_hz,
(double)rate, (double)rate,
(double)rate_hz, (double)rate_hz,
(double)scaled_frame_time_us); (double)scaled_frame_time_us);

View File

@ -59,7 +59,7 @@ void CRRCSim::send_servos_heli(const struct sitl_input &input)
float roll_rate = (swash1 - swash2)/2; float roll_rate = (swash1 - swash2)/2;
float pitch_rate = -((swash1 + swash2)/2.0 - swash3)/2; float pitch_rate = -((swash1 + swash2)/2.0 - swash3)/2;
float yaw_rate = -(tail_rotor - 0.5); float yaw_rate = -(tail_rotor - 0.5);
servo_packet pkt; servo_packet pkt;
pkt.roll_rate = constrain_float(roll_rate, -0.5, 0.5); pkt.roll_rate = constrain_float(roll_rate, -0.5, 0.5);
pkt.pitch_rate = constrain_float(pitch_rate, -0.5, 0.5); pkt.pitch_rate = constrain_float(pitch_rate, -0.5, 0.5);

View File

@ -52,7 +52,7 @@ private:
float yaw_rate; float yaw_rate;
float col_pitch; float col_pitch;
}; };
/* /*
reply packet sent from CRRCSim to ArduPilot reply packet sent from CRRCSim to ArduPilot
*/ */

View File

@ -48,7 +48,7 @@ private:
struct servo_packet { struct servo_packet {
float motor_speed[4]; float motor_speed[4];
}; };
/* /*
reply packet sent from Gazebo to ArduPilot reply packet sent from Gazebo to ArduPilot
*/ */

View File

@ -57,8 +57,8 @@ void Gimbal::update(void)
Matrix3f vehicle_dcm; Matrix3f vehicle_dcm;
vehicle_dcm.from_euler(radians(fdm.rollDeg), radians(fdm.pitchDeg), radians(fdm.yawDeg)); vehicle_dcm.from_euler(radians(fdm.rollDeg), radians(fdm.pitchDeg), radians(fdm.yawDeg));
Vector3f vehicle_gyro = Vector3f(radians(fdm.rollRate), Vector3f vehicle_gyro = Vector3f(radians(fdm.rollRate),
radians(fdm.pitchRate), radians(fdm.pitchRate),
radians(fdm.yawRate)); radians(fdm.yawRate));
Vector3f vehicle_accel_body = Vector3f(fdm.xAccel, fdm.yAccel, fdm.zAccel); Vector3f vehicle_accel_body = Vector3f(fdm.xAccel, fdm.yAccel, fdm.zAccel);
@ -79,7 +79,7 @@ void Gimbal::update(void)
Matrix3f rotmat_copter_gimbal = dcm.transposed() * vehicle_dcm; Matrix3f rotmat_copter_gimbal = dcm.transposed() * vehicle_dcm;
joint_angles = rotmat_copter_gimbal.transposed().to_euler312(); joint_angles = rotmat_copter_gimbal.transposed().to_euler312();
/* 4) For each of the three joints, calculate upper and lower rate limits /* 4) For each of the three joints, calculate upper and lower rate limits
from the corresponding angle limits and current joint angles from the corresponding angle limits and current joint angles
@ -145,7 +145,7 @@ void Gimbal::update(void)
// in an inertial frame of reference // in an inertial frame of reference
// demandedGimbalRatesInertial(X,Y,Z) = relativeGimbalRate(X,Y,Z) + copterAngRate_G // demandedGimbalRatesInertial(X,Y,Z) = relativeGimbalRate(X,Y,Z) + copterAngRate_G
// Vector3f demandedGimbalRatesInertial = relativeGimbalRate + copterAngRate_G; // Vector3f demandedGimbalRatesInertial = relativeGimbalRate + copterAngRate_G;
// for the moment we will set gyros equal to demanded_angular_rate // for the moment we will set gyros equal to demanded_angular_rate
gimbal_angular_rate = demRateRaw; // demandedGimbalRatesInertial + true_gyro_bias - supplied_gyro_bias gimbal_angular_rate = demRateRaw; // demandedGimbalRatesInertial + true_gyro_bias - supplied_gyro_bias
@ -198,7 +198,7 @@ void Gimbal::send_report(void)
mavlink_message_t msg; mavlink_message_t msg;
mavlink_status_t status; mavlink_status_t status;
if (mavlink_frame_char_buffer(&mavlink.rxmsg, &mavlink.status, if (mavlink_frame_char_buffer(&mavlink.rxmsg, &mavlink.status,
buf[i], buf[i],
&msg, &status) == MAVLINK_FRAMING_OK) { &msg, &status) == MAVLINK_FRAMING_OK) {
switch (msg.msgid) { switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: { case MAVLINK_MSG_ID_HEARTBEAT: {
@ -249,8 +249,8 @@ void Gimbal::send_report(void)
mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);
uint8_t saved_seq = chan0_status->current_tx_seq; uint8_t saved_seq = chan0_status->current_tx_seq;
chan0_status->current_tx_seq = mavlink.seq; chan0_status->current_tx_seq = mavlink.seq;
len = mavlink_msg_heartbeat_encode(vehicle_system_id, len = mavlink_msg_heartbeat_encode(vehicle_system_id,
vehicle_component_id, vehicle_component_id,
&msg, &heartbeat); &msg, &heartbeat);
chan0_status->current_tx_seq = saved_seq; chan0_status->current_tx_seq = saved_seq;
@ -282,13 +282,13 @@ void Gimbal::send_report(void)
mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0);
uint8_t saved_seq = chan0_status->current_tx_seq; uint8_t saved_seq = chan0_status->current_tx_seq;
chan0_status->current_tx_seq = mavlink.seq; chan0_status->current_tx_seq = mavlink.seq;
len = mavlink_msg_gimbal_report_encode(vehicle_system_id, len = mavlink_msg_gimbal_report_encode(vehicle_system_id,
vehicle_component_id, vehicle_component_id,
&msg, &gimbal_report); &msg, &gimbal_report);
chan0_status->current_tx_seq = saved_seq; chan0_status->current_tx_seq = saved_seq;
mav_socket.send(&msg.magic, len); mav_socket.send(&msg.magic, len);
delta_velocity.zero(); delta_velocity.zero();
delta_angle.zero(); delta_angle.zero();
} }

View File

@ -71,7 +71,7 @@ private:
// reporting period in ms // reporting period in ms
const float reporting_period_ms; const float reporting_period_ms;
// integral of gyro vector over last time interval. In radians // integral of gyro vector over last time interval. In radians
Vector3f delta_angle; Vector3f delta_angle;

View File

@ -87,14 +87,14 @@ void Helicopter::update(const struct sitl_input &input)
// simulate a traditional helicopter // simulate a traditional helicopter
float tail_rotor = (input.servos[3]-1000) / 1000.0f; float tail_rotor = (input.servos[3]-1000) / 1000.0f;
thrust = (rsc/rsc_setpoint) * (swash1+swash2+swash3) / 3.0f; thrust = (rsc/rsc_setpoint) * (swash1+swash2+swash3) / 3.0f;
torque_effect_accel = (rsc_scale+thrust) * rotor_rot_accel; torque_effect_accel = (rsc_scale+thrust) * rotor_rot_accel;
roll_rate = swash1 - swash2; roll_rate = swash1 - swash2;
pitch_rate = (swash1+swash2) / 2.0f - swash3; pitch_rate = (swash1+swash2) / 2.0f - swash3;
yaw_rate = tail_rotor - 0.5f; yaw_rate = tail_rotor - 0.5f;
lateral_y_thrust = yaw_rate * rsc_scale * tail_thrust_scale; lateral_y_thrust = yaw_rate * rsc_scale * tail_thrust_scale;
break; break;
} }
@ -105,7 +105,7 @@ void Helicopter::update(const struct sitl_input &input)
float swash4 = (input.servos[3]-1000) / 1000.0f; float swash4 = (input.servos[3]-1000) / 1000.0f;
float swash5 = (input.servos[4]-1000) / 1000.0f; float swash5 = (input.servos[4]-1000) / 1000.0f;
float swash6 = (input.servos[5]-1000) / 1000.0f; float swash6 = (input.servos[5]-1000) / 1000.0f;
thrust = (rsc / rsc_setpoint) * (swash1+swash2+swash3+swash4+swash5+swash6) / 6.0f; thrust = (rsc / rsc_setpoint) * (swash1+swash2+swash3+swash4+swash5+swash6) / 6.0f;
torque_effect_accel = (rsc_scale + rsc / rsc_setpoint) * rotor_rot_accel * ((swash1+swash2+swash3) - (swash4+swash5+swash6)); torque_effect_accel = (rsc_scale + rsc / rsc_setpoint) * rotor_rot_accel * ((swash1+swash2+swash3) - (swash4+swash5+swash6));
@ -120,7 +120,7 @@ void Helicopter::update(const struct sitl_input &input)
float right_rotor = (input.servos[3]-1000) / 1000.0f; float right_rotor = (input.servos[3]-1000) / 1000.0f;
float left_rotor = (input.servos[4]-1000) / 1000.0f; float left_rotor = (input.servos[4]-1000) / 1000.0f;
thrust = (rsc/rsc_setpoint) * (swash1+swash2+swash3) / 3.0f; thrust = (rsc/rsc_setpoint) * (swash1+swash2+swash3) / 3.0f;
torque_effect_accel = (rsc_scale+thrust) * rotor_rot_accel; torque_effect_accel = (rsc_scale+thrust) * rotor_rot_accel;

View File

@ -122,7 +122,7 @@ bool JSBSim::create_templates(void)
if (f == NULL) { if (f == NULL) {
hal.scheduler->panic("Unable to create jsbsim fgout script"); hal.scheduler->panic("Unable to create jsbsim fgout script");
} }
fprintf(f, "<?xml version=\"1.0\"?>\n" fprintf(f, "<?xml version=\"1.0\"?>\n"
"<output name=\"127.0.0.1\" type=\"FLIGHTGEAR\" port=\"%u\" protocol=\"udp\" rate=\"1000\"/>\n", "<output name=\"127.0.0.1\" type=\"FLIGHTGEAR\" port=\"%u\" protocol=\"udp\" rate=\"1000\"/>\n",
fdm_port); fdm_port);
fclose(f); fclose(f);
@ -135,7 +135,7 @@ bool JSBSim::create_templates(void)
} }
float r, p, y; float r, p, y;
dcm.to_euler(&r, &p, &y); dcm.to_euler(&r, &p, &y);
fprintf(f, fprintf(f,
"<?xml version=\"1.0\"?>\n" "<?xml version=\"1.0\"?>\n"
"<initialize name=\"Start up location\">\n" "<initialize name=\"Start up location\">\n"
" <latitude unit=\"DEG\"> %f </latitude>\n" " <latitude unit=\"DEG\"> %f </latitude>\n"
@ -172,7 +172,7 @@ bool JSBSim::start_JSBSim(void)
int p[2]; int p[2];
int devnull = open("/dev/null", O_RDWR); int devnull = open("/dev/null", O_RDWR);
if (pipe(p) != 0) { if (pipe(p) != 0) {
hal.scheduler->panic("Unable to create pipe"); hal.scheduler->panic("Unable to create pipe");
} }
pid_t child_pid = fork(); pid_t child_pid = fork();
if (child_pid == 0) { if (child_pid == 0) {
@ -194,8 +194,8 @@ bool JSBSim::start_JSBSim(void)
exit(1); exit(1);
} }
int ret = execlp("JSBSim", int ret = execlp("JSBSim",
"JSBSim", "JSBSim",
"--realtime", "--realtime",
"--suspend", "--suspend",
"--nice", "--nice",
@ -218,7 +218,7 @@ bool JSBSim::start_JSBSim(void)
} }
if (!expect("JSBSim Execution beginning")) { if (!expect("JSBSim Execution beginning")) {
hal.scheduler->panic("Failed to start JSBSim"); hal.scheduler->panic("Failed to start JSBSim");
} }
if (!open_control_socket()) { if (!open_control_socket()) {
hal.scheduler->panic("Failed to open JSBSim control socket"); hal.scheduler->panic("Failed to open JSBSim control socket");
@ -252,7 +252,7 @@ bool JSBSim::expect(const char *str)
{ {
const char *basestr = str; const char *basestr = str;
while (*str) { while (*str) {
char c; char c;
if (read(jsbsim_stdout, &c, 1) != 1) { if (read(jsbsim_stdout, &c, 1) != 1) {
return false; return false;
} }
@ -283,7 +283,7 @@ bool JSBSim::open_control_socket(void)
sock_control.set_blocking(false); sock_control.set_blocking(false);
opened_control_socket = true; opened_control_socket = true;
char startup[] = char startup[] =
"info\n" "info\n"
"resume\n" "resume\n"
"step\n" "step\n"
@ -337,7 +337,7 @@ void JSBSim::send_servos(const struct sitl_input &input)
rudder = (ch2+ch1)/2.0f; rudder = (ch2+ch1)/2.0f;
} }
float wind_speed_fps = input.wind.speed / FEET_TO_METERS; float wind_speed_fps = input.wind.speed / FEET_TO_METERS;
asprintf(&buf, asprintf(&buf,
"set fcs/aileron-cmd-norm %f\n" "set fcs/aileron-cmd-norm %f\n"
"set fcs/elevator-cmd-norm %f\n" "set fcs/elevator-cmd-norm %f\n"
"set fcs/rudder-cmd-norm %f\n" "set fcs/rudder-cmd-norm %f\n"
@ -370,7 +370,7 @@ void JSBSim::send_servos(const struct sitl_input &input)
/* nasty hack .... /* nasty hack ....
JSBSim sends in little-endian JSBSim sends in little-endian
*/ */
void FGNetFDM::ByteSwap(void) void FGNetFDM::ByteSwap(void)
{ {
uint32_t *buf = (uint32_t *)this; uint32_t *buf = (uint32_t *)this;
for (uint16_t i=0; i<sizeof(*this)/4; i++) { for (uint16_t i=0; i<sizeof(*this)/4; i++) {

View File

@ -109,7 +109,7 @@ public:
float phidot; // roll rate (radians/sec) float phidot; // roll rate (radians/sec)
float thetadot; // pitch rate (radians/sec) float thetadot; // pitch rate (radians/sec)
float psidot; // yaw rate (radians/sec) float psidot; // yaw rate (radians/sec)
float vcas; // calibrated airspeed float vcas; // calibrated airspeed
float climb_rate; // feet per second float climb_rate; // feet per second
float v_north; // north velocity in local/body frame, fps float v_north; // north velocity in local/body frame, fps
float v_east; // east velocity in local/body frame, fps float v_east; // east velocity in local/body frame, fps
@ -131,17 +131,17 @@ public:
float slip_deg; // slip ball deflection float slip_deg; // slip ball deflection
// Pressure // Pressure
// Engine status // Engine status
uint32_t num_engines; // Number of valid engines uint32_t num_engines; // Number of valid engines
uint32_t eng_state[FG_MAX_ENGINES];// Engine state (off, cranking, running) uint32_t eng_state[FG_MAX_ENGINES];// Engine state (off, cranking, running)
float rpm[FG_MAX_ENGINES]; // Engine RPM rev/min float rpm[FG_MAX_ENGINES]; // Engine RPM rev/min
float fuel_flow[FG_MAX_ENGINES]; // Fuel flow gallons/hr float fuel_flow[FG_MAX_ENGINES]; // Fuel flow gallons/hr
float fuel_px[FG_MAX_ENGINES]; // Fuel pressure psi float fuel_px[FG_MAX_ENGINES]; // Fuel pressure psi
float egt[FG_MAX_ENGINES]; // Exhuast gas temp deg F float egt[FG_MAX_ENGINES]; // Exhuast gas temp deg F
float cht[FG_MAX_ENGINES]; // Cylinder head temp deg F float cht[FG_MAX_ENGINES]; // Cylinder head temp deg F
float mp_osi[FG_MAX_ENGINES]; // Manifold pressure float mp_osi[FG_MAX_ENGINES]; // Manifold pressure
float tit[FG_MAX_ENGINES]; // Turbine Inlet Temperature float tit[FG_MAX_ENGINES]; // Turbine Inlet Temperature
float oil_temp[FG_MAX_ENGINES]; // Oil temp deg F float oil_temp[FG_MAX_ENGINES]; // Oil temp deg F
float oil_px[FG_MAX_ENGINES]; // Oil pressure psi float oil_px[FG_MAX_ENGINES]; // Oil pressure psi

View File

@ -42,7 +42,7 @@ void Tracker::update_position_servos(float delta_time, float &yaw_rate, float &p
/* /*
update function for onoff servos. update function for onoff servos.
These servos either move at a constant rate or are still These servos either move at a constant rate or are still
Returns (yaw_rate,pitch_rate) tuple Returns (yaw_rate,pitch_rate) tuple
*/ */
void Tracker::update_onoff_servos(float &yaw_rate, float &pitch_rate) void Tracker::update_onoff_servos(float &yaw_rate, float &pitch_rate)
@ -71,7 +71,7 @@ void Tracker::update(const struct sitl_input &input)
{ {
// how much time has passed? // how much time has passed?
float delta_time = frame_time_us * 1.0e-6f; float delta_time = frame_time_us * 1.0e-6f;
float yaw_rate, pitch_rate; float yaw_rate, pitch_rate;
yaw_input = (input.servos[0]-1500)/500.0f; yaw_input = (input.servos[0]-1500)/500.0f;
@ -117,8 +117,8 @@ void Tracker::update(const struct sitl_input &input)
last_debug_us = time_now_us; last_debug_us = time_now_us;
printf("roll=%.1f pitch=%.1f yaw=%.1f rates=%.1f/%.1f/%.1f in=%.3f,%.3f\n", printf("roll=%.1f pitch=%.1f yaw=%.1f rates=%.1f/%.1f/%.1f in=%.3f,%.3f\n",
roll_current, roll_current,
pitch_current_relative, pitch_current_relative,
yaw_current_relative, yaw_current_relative,
roll_rate, pitch_rate, yaw_rate, roll_rate, pitch_rate, yaw_rate,
yaw_input, pitch_input); yaw_input, pitch_input);
} }

View File

@ -50,7 +50,7 @@ private:
struct servo_packet { struct servo_packet {
uint16_t servos[16]; uint16_t servos[16];
}; };
/* /*
reply packet sent from last_letter to ArduPilot reply packet sent from last_letter to ArduPilot
*/ */

View File

@ -95,9 +95,9 @@ void SITL::simstate_send(mavlink_channel_t chan)
state.xAccel, state.xAccel,
state.yAccel, state.yAccel,
state.zAccel, state.zAccel,
radians(state.rollRate), radians(state.rollRate),
radians(state.pitchRate), radians(state.pitchRate),
radians(state.yawRate), radians(state.yawRate),
state.latitude*1.0e7, state.latitude*1.0e7,
state.longitude*1.0e7); state.longitude*1.0e7);
} }