Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: fixed whitespace on flightaxis code

This commit is contained in:
Andrew Tridgell 2017-02-13 20:45:38 +11:00
parent 83d055eceb
commit d338d9c0aa
2 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -116,13 +116,13 @@ char *FlightAxis::soap_request(const char *action, const char *fmt, ...)
{
va_list ap;
char *req1;
va_start(ap, fmt);
vasprintf(&req1, fmt, ap);
va_end(ap);
//printf("%s\n", req1);
// open SOAP socket to FlightAxis
SocketAPM sock(false);
if (!sock.connect(controller_ip, controller_port)) {
@ -183,7 +183,7 @@ Connection: Keep-Alive
}
return strdup(reply);
}
void FlightAxis::exchange_data(const struct sitl_input &input)
@ -223,7 +223,7 @@ void FlightAxis::exchange_data(const struct sitl_input &input)
memcpy(&scaled_servos[0], &scaled_servos[4], sizeof(saved));
memcpy(&scaled_servos[4], saved, sizeof(saved));
}
if (heli_demix) {
// FlightAxis expects "roll/pitch/collective/yaw" input
float swash1 = scaled_servos[0];
@ -236,8 +236,8 @@ void FlightAxis::exchange_data(const struct sitl_input &input)
scaled_servos[0] = constrain_float(roll_rate + 0.5, 0, 1);
scaled_servos[1] = constrain_float(pitch_rate + 0.5, 0, 1);
}
char *reply = soap_request("ExchangeData", R"(<?xml version='1.0' encoding='UTF-8'?><soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>
<soap:Body>
<ExchangeData>
@ -271,8 +271,8 @@ void FlightAxis::exchange_data(const struct sitl_input &input)
free(reply);
}
}
/*
update the FlightAxis simulation by one time step
*/
@ -308,7 +308,7 @@ void FlightAxis::update(const struct sitl_input &input)
quat.rotation_matrix(dcm);
gyro = Vector3f(radians(constrain_float(state.m_rollRate_DEGpSEC, -2000, 2000)),
radians(constrain_float(state.m_pitchRate_DEGpSEC, -2000, 2000)),
radians(constrain_float(state.m_pitchRate_DEGpSEC, -2000, 2000)),
-radians(constrain_float(state.m_yawRate_DEGpSEC, -2000, 2000))) * target_speedup;
velocity_ef = Vector3f(state.m_velocityWorldU_MPS,
@ -329,20 +329,20 @@ void FlightAxis::update(const struct sitl_input &input)
gyro.rotate(rotation);
accel_body.rotate(rotation);
}
// accel on the ground is nasty in realflight, and prevents helicopter disarm
if (state.m_isTouchingGround) {
Vector3f accel_ef = (velocity_ef - last_velocity_ef) / dt_seconds;
accel_ef.z -= GRAVITY_MSS;
accel_body = dcm.transposed() * accel_ef;
}
// limit to 16G to match pixhawk
float a_limit = GRAVITY_MSS*16;
accel_body.x = constrain_float(accel_body.x, -a_limit, a_limit);
accel_body.y = constrain_float(accel_body.y, -a_limit, a_limit);
accel_body.z = constrain_float(accel_body.z, -a_limit, a_limit);
// offset based on first position to account for offset in RF world
if (position_offset.is_zero() || state.m_resetButtonHasBeenPressed) {
position_offset = position;
@ -377,7 +377,7 @@ void FlightAxis::update(const struct sitl_input &input)
}
last_frame_count_s = state.m_currentPhysicsTime_SEC;
}
last_time_s = state.m_currentPhysicsTime_SEC;
last_velocity_ef = velocity_ef;

6
libraries/SITL/SIM_FlightAxis.h
View File

@ -90,7 +90,7 @@ public:
} state;
static const uint16_t num_keys = sizeof(state)/sizeof(double);
struct keytable {
const char *key;
double &ref;
@ -150,7 +150,7 @@ public:
{ "m-flightAxisControllerIsActive", state.m_flightAxisControllerIsActive },
{ "m-resetButtonHasBeenPressed", state.m_resetButtonHasBeenPressed },
};
private:
char *soap_request(const char *action, const char *fmt, ...);
void exchange_data(const struct sitl_input &input);
@ -168,7 +168,7 @@ private:
Vector3f last_velocity_ef;
Matrix3f att_rotation;
enum Rotation rotation = ROTATION_NONE;
const char *controller_ip = "127.0.0.1";
uint16_t controller_port = 18083;
};