mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-05 15:33:57 -04:00
SITL: remove trailing whitespace
This commit is contained in:
parent
49a42dc985
commit
f0a20cdc7c
@ -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);
|
||||||
|
@ -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);
|
||||||
|
@ -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
|
||||||
*/
|
*/
|
||||||
|
@ -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
|
||||||
*/
|
*/
|
||||||
|
@ -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();
|
||||||
}
|
}
|
||||||
|
@ -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;
|
||||||
|
|
||||||
|
@ -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;
|
||||||
|
|
||||||
|
@ -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++) {
|
||||||
|
@ -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
|
||||||
|
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
|
@ -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
|
||||||
*/
|
*/
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user