int main(int ac, char** av)
{
#if 1
{
fixed_t fu = float_to_fixed(1.2);
fixed_t bar = float_to_fixed(2.4);
fixed_t baz = fixed_add(fu, bar);
printf("%f + ", fixed_to_float(fu));
printf("%f = ", fixed_to_float(bar));
printf("%f\n", fixed_to_float(baz));
}
{
fixed_t fu = float_to_fixed(1.5);
fixed_t bar = int_to_fixed(2);
fixed_t baz = fixed_mul(fu, bar);
printf("%f * ", fixed_to_float(fu));
printf("%f = ", fixed_to_float(bar));
printf("%f\n", fixed_to_float(baz));
}
{
fixed_t fu = int_to_fixed(1);
fixed_t bar = int_to_fixed(2);
fixed_t baz = fixed_div(fu, bar);
printf("%f / ", fixed_to_float(fu));
printf("%f = ", fixed_to_float(bar));
printf("%f\n", fixed_to_float(baz));
}
{
fixed_t fu = int_to_fixed(-1);
fixed_t bar = int_to_fixed(2);
fixed_t baz = fixed_div(fu, bar);
printf("%f / ", fixed_to_float(fu));
printf("%f = ", fixed_to_float(bar));
printf("%f\n", fixed_to_float(baz));
}
#endif
#if 0
{
fixed_t alpha = float_to_fixed(0.0);
fixed_t step = float_to_fixed(0.01);
for (; alpha < FIXED_TWO_PI; alpha = fixed_add(alpha, step))
{
printf("%f ", fixed_to_float(alpha));
printf("%f\n", fabsf(sinf(fixed_to_float(alpha)) - fixed_to_float(fixed_sin(alpha))));
}
}
#endif
return 0;
}
/**
A compliant controller for the motor.
Similar to a PD-position controller. To use, the user should call @ref mc_set_stiffness,
@ref mc_set_dampness, and @ref mc_set_command_current to set the parameters of the controller.
A call to @ref mc_set_command_current will latch the new stiffness and dampness values; without this
call, dampness and stiffness (and the command current) won't change. The formula used in the
control is @code Icontrol = Icommand - (stiffness * pos) - (dampness * vel) @endcode.
Icontrol is passed to the PID current controller, which will attempt to go to the desired current.
To request a specific position (in radians) with a given stiffness, use: @code Icommand = stiffness * desired_position @endcode.
*/
void mc_compliant_control(void){
float pos_rads = mc_data.get_position();
fixed pos = float_to_fixed(pos_rads);
fixed vel = float_to_fixed(mc_data.get_velocity());
fixed control = mc_command_current - fixed_mult(mc_c1, pos) - fixed_mult(mc_c2, vel);
mc_set_target_current_fixed(control);
mc_pid_current();
}
/**
Sets the command current used by @ref mc_compliant_control to @c new_command.
Also latches new values for stiffness and dampness.
@param new_command The command current for the compliant controller.
*/
void mc_set_command_current(float new_command)
{
mc_command_current = float_to_fixed(new_command);
mc_save_comm = new_command;
//if (new_command != 7.2){mcu_led_red_blink(10);}
mc_c1 = mc_new_c1;
mc_c2 = mc_new_c2;
}
/**
Calculates the result of plugging in @c value into the linear equation
described by the @c offset and @c coeff as a fixed point value.
Used in converting between units.
@code retval = (coeff * value) + offset @endcode
@param offset The offset of the linear equation. Often called @c b.
@param coeff The coefficient of the linear equation. Often called @c m.
@param value The value to be plugged into the linear equation. Often called @c x.
@return The fixed point result of plugging @c value into the linear equation.
*/
fixed linear_to_fixed(int offset, float coeff, int value){
int a, b, c, d, e;
a = value;
b = a - offset; //subtract the offset
c = -1 * int_to_fixed(b); //convert to fixed point
d = float_to_fixed(coeff); //convert coefficient to fixed point
e = fixed_mult(c,d); //multiply gain to get amps in fixed point
return e;
}
void buildSinTable(short* pTbl) {
int i;
pSinTable = pTbl;
for (i = 0;i < SINTABLE_LENGTH;++i) {
const float s = taylor_sin(F_PI * (float)i / (float)SINTABLE_LENGTH);
pSinTable[i] = (short) float_to_fixed(s);
}
}
real& real::operator=(const GLfloat& flt)
{
if (_is_float)
{
_float = flt;
}
else
{
_fixed = float_to_fixed(flt);
}
return *this;
}
GLfixed real::as_fixed() const
{
return _is_float ? float_to_fixed(_float) : _fixed;
}
GLfixed const_real_ptr::as_fixed(size_t n) const
{
return _is_float ? float_to_fixed(_float[n]) : _fixed[n];
}
/**
Sets the dampness used by @ref mc_compliant_control to @c new_c2.
This value will not be used (latched) until @ref mc_set_command_current has been called.
@param new_c2 The new dampness for the compliant controller.
*/
void mc_set_dampness(float new_c2){
mc_new_c2 = float_to_fixed(new_c2);
mc_save_damp = new_c2;
//if (new_c2-0.2 > 0.01 || new_c2-0.2 < -0.01){mcu_led_red_blink(10);}else{mcu_led_green_blink(10);}
}
/**
Sets the stiffness used by @ref mc_compliant_control to @c new_c1.
This value will not be used (latched) until @ref mc_set_command_current has been called.
@param new_c1 The new stiffness for the compliant controller.
*/
void mc_set_stiffness(float new_c1){
mc_new_c1 = float_to_fixed(new_c1);
mc_save_stiff = new_c1;
//if (new_c1 != 4.0){mcu_led_red_blink(10);}
}
/**
Sets the target current to @c new_current without any limiting.
@param new_current The new unlimited target current.
*/
void mc_set_unsafe_target_current(float new_current)
{
fixed curr = float_to_fixed(new_current);
mc_target_current = /*mc_data.operation * */curr;
}
/**
Sets the target current of the PID current controller to @c new_current.
The current controller will attempt to achieve this current on the next call to @ref mc_pid_current.
@param new_current The target current for the PID current controller. @c target_min < @c new_current < @c target_max.
*/
void mc_set_target_current(float new_current)
{
fixed curr = float_to_fixed(new_current);
mc_set_target_current_fixed(curr);
}
/**
Initializes the motor controller. Must be called before the motor
controller module can be used properly.
@param max_volts The maximum voltage the motor can take.
@param max_current The maximum allowed current for the motor.
@param min_current The minimum allowed current for the motor, often just -1 * @c max_current.
@param max_target The maximum allowed current to be commanded using @ref mc_set_target_current.
@param min_target The minimum allowed current to be commanded using @ref mc_set_target_current.
@param thermal_current_limit The nominal or max continuous current for the motor.
@param thermal_time_constant The Winding Thermal Time Constant.
@param kp Proportional constant for the PID current controller.
@param ki Integral constant for the PID current controller.
@param kd Derivative constant for the PID current controller.
@param max_pwm Maximum allowed PWM for the motor, often to limit voltage.
@param error_limit If there are this many errors in a row, shutdown the motor. (DEPRECATED)
@param smart_error_limit If the integral of the error is greater than this value, shutdown the motor.
@param op The operation of the motor, either @c MC_NORMAL or @c MC_BACKWARDS. Used if positive PWM and positive position don't match.
@param current A function that returns the motor current as a fixed point value in amps.
@param position A function that returns the motor position as a floating point number in radians.
@param velocity A function that returns the motor velocity as a floating point number in radians.
*/
void mc_init(
float max_volts,
float max_current,
float min_current,
float max_target, // dont see this in soft_setup_init
float min_target, // dont see this in soft_setup_init
float thermal_current_limit, //from spec sheet
float thermal_time_constant, //tau, from spec sheet
float kp,
float ki,
float kd,
int max_pwm,
int error_limit, // set 4 now
float smart_error_limit, // set 10 now
MC_OPERATION op, // I dont see this in soft_setup_init
FIXED_VOID_F current,
FLOAT_VOID_F position,
FLOAT_VOID_F velocity) //initializes the motor controller
{
mc_data.operation = op;
mc_data.motor_voltage_max = max_volts; //Volts
mc_data.current_max = float_to_fixed(max_current); //Amps
mc_data.current_min = float_to_fixed(min_current); //Amps
mc_data.target_max = float_to_fixed(max_target);
mc_data.target_min = float_to_fixed(min_target);
mc_data.thermal_limit = float_to_fixed((thermal_current_limit*thermal_current_limit)*thermal_time_constant);
mc_data.tau_inv = float_to_fixed(1.0/thermal_time_constant);
mc_data.thermal_safe = fixed_mult(float_to_fixed(0.75), mc_data.thermal_limit);
mc_data.thermal_diff = float_to_fixed(1.0/(thermal_current_limit - fixed_to_float(mc_data.thermal_safe)));
mc_data.dt = float_to_fixed(1.0/(float)(1000*SCHED_SPEED));
mc_data.kp = float_to_fixed(kp);
mc_data.ki = float_to_fixed(ki);
mc_data.kd = float_to_fixed(kd);
mc_data.integral = 0;
mc_data.max_pwm = max_pwm;
mc_data.max_integral = float_to_fixed((((float)max_pwm)/ki));
mc_data.error_limit = error_limit;
mc_data.smart_error_limit = float_to_fixed(smart_error_limit);
mc_data.smart_error_limit_inverse = float_to_fixed(1.0*10.0/smart_error_limit);
mc_data.get_current = current;
mc_data.get_position = position;
mc_data.get_velocity = velocity;
}
int main(void)
{
int sock, sock2;
struct sockaddr_in echoclient,from, ctrlclient, ctrlinclient;
unsigned int echolen, clientlen;
int received = 0;
FGNetFDM buf;
char* buffer = (char*)&buf;
FGNetCtrls bufctrl;
char* buffer_ctrl = (char*)&bufctrl;
struct known_state state;
struct known_state old_state;
struct known_state_ints state_ints;
struct known_state_ints old_state_ints;
srand ( time(NULL) );
memset(&state, 0, sizeof(state));
memset(&old_state, 0, sizeof(state));
memset(&state_ints, 0, sizeof(state_ints));
memset(&old_state_ints, 0, sizeof(state_ints));
/* Create the UDP socket */
if ((sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) {
std::cerr << "Failed to create socket" << std::endl;
perror("socket");
return -1;
}
if ((sock2 = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) {
std::cerr << "Failed to create socket" << std::endl;
perror("socket");
return -1;
}
/// the port where net fdm is received
memset(&echoclient, 0, sizeof(echoclient)); /* Clear struct */
echoclient.sin_family = AF_INET; /* Internet/IP */
echoclient.sin_addr.s_addr = inet_addr("127.0.0.1"); /* IP address */
echoclient.sin_port = htons(5500); /* server port */
/// the port where the modified net ctrl is sent to flightgear
memset(&ctrlclient, 0, sizeof(ctrlclient)); /* Clear struct */
ctrlclient.sin_family = AF_INET; /* Internet/IP */
ctrlclient.sin_addr.s_addr = inet_addr("127.0.0.1"); /* IP address */
ctrlclient.sin_port = htons(5600); /* server port */
/// this is the port where the net ctrl comes from flight gear
memset(&ctrlinclient, 0, sizeof(ctrlinclient)); /* Clear struct */
ctrlinclient.sin_family = AF_INET; /* Internet/IP */
ctrlinclient.sin_addr.s_addr = inet_addr("127.0.0.1"); /* IP address */
ctrlinclient.sin_port = htons(5700); /* server port */
int length = sizeof(buf);
std::cout << "packet length should be " << length << " bytes" << std::endl;
echolen = length;
char errorbuf[100];
/// need this?
int ret = bind(sock, (struct sockaddr*)&echoclient, sizeof(echoclient) );
int ret2 = bind(sock2, (struct sockaddr*)&ctrlinclient, sizeof(ctrlinclient) );
std::cout << "bind " << ret << " " << ret2 << std::endl;
///////////////////////////////////////////////////////////////////
std::string file = "telemetry.csv";
std::ofstream telem(file.c_str(), std::ios_base::out );
if (!telem) {
std::cout << "File \"" << file << "\" not found.\n";
throw fileNotFound();
return 2;
}
telem.precision(10);
telem <<
"longitude, latitude, altitude" <<
"p, q, r," <<
"elevator, rudder, aileron" <<
std::endl;
//std::string binfile = "sim_telemetry.bin";
//std::ofstream myFile(binfile, std::ios::out | std::ios::binary);
double time = 0.0;
float dist = 40e3; //state.altitude;
float div = 3e7;
double target_longitude = -2.137;
double target_latitude = .658;
double target_altitude = 50.0; //meters
std::cout << "struct size " << sizeof(state) << std::endl;
FILE* telem_file;
telem_file = fopen("telem.bin","wb");
int i = 0;
while(1) {
i++;
// I think this clobbers echoclient
/// receive net_fdm over udp
received = recvfrom(sock, buffer, sizeof(buf), 0,
(struct sockaddr *) &from,
&echolen);
/// -extract accelerations A_X_pilot etc, add noise
/// - are phidot, thetadot, psidot in body frame? Probably
/// (I thought someone told me there was no difference, but that doesn't
/// make sense- what if the vehicle was 90 degrees up from normal, how is
/// roll still roll?)
/// - take position lat long alt, but filter so it is only updated at a few Hz
/// and that's all we'll know
FGNetFDM2Props( &buf);
if (i == 1) {
target_longitude = buf.longitude + (float)(rand()%(int)dist)/div - dist/div*0.5;
target_latitude = buf.latitude + (float)(rand()%(int)dist)/div - dist/div*0.5;
std::cout << "t longitude=" << target_longitude << ", t latitude=" << target_latitude << std::endl;
}
if (i%10 == 0) {
state.longitude= (float)((int)(180e4/M_PI*buf.longitude))/(180e4/M_PI);;
state.latitude = (float)((int)(180e4/M_PI*buf.latitude))/(180e4/M_PI);
state.altitude = buf.altitude*M2FT;
} else {
state.longitude= old_state.longitude;
state.latitude = old_state.latitude;
state.altitude = old_state.altitude;
}
state.p = FPFIX(buf.p);
state.q = FPFIX(buf.q);
state.r = FPFIX(buf.r);
state.A_X_pilot = FPFIX(buf.A_X_pilot);
state.A_Y_pilot = FPFIX(buf.A_Y_pilot);
state.A_Z_pilot = FPFIX(buf.A_Z_pilot);
state.target_long= target_longitude;
state.target_lat = target_latitude;
state.target_alt = target_altitude;
#if 0
/// fixed point version
if (j%10 == 0) {
/// convert to arc-minutes
state_ints.longitude = float_to_fixed(180.0*60.0/M_PI*buf.longitude);
state_ints.latitude = float_to_fixed(180.0*60.0/M_PI*buf.latitude);
/// convert to feet later?
state_ints.altitude = (int)(buf.altitude);
/// TEMP
/// alternate way of getting velocity- this one matches the sim
double x1 = (EARTH_RADIUS_METERS+state.altitude)*cos(state.latitude)*cos(state.longitude);
double y1 = (EARTH_RADIUS_METERS+state.altitude)*cos(state.latitude)*sin(state.longitude);
double z1 = (EARTH_RADIUS_METERS+state.altitude)*sin(state.latitude);
double x2 = (EARTH_RADIUS_METERS+old_state.altitude)*cos(old_state.latitude)*cos(old_state.longitude);
double y2 = (EARTH_RADIUS_METERS+old_state.altitude)*cos(old_state.latitude)*sin(old_state.longitude);
double z2 = (EARTH_RADIUS_METERS+old_state.altitude)*sin(old_state.latitude);
// delta xyz to target
double xt = (EARTH_RADIUS_METERS+target_altitude)*cos(target_latitude)*cos(target_longitude);
double yt = (EARTH_RADIUS_METERS+target_altitude)*cos(target_latitude)*sin(target_longitude);
double zt = (EARTH_RADIUS_METERS+target_altitude)*sin(target_latitude);
double dx = (x1-x2);
double dy = (y1-y2);
double dz = (z1-z2);
// length of vector pointing from old location to current
float l1 = sqrtf(dx*dx + dy*dy + dz*dz);
/// length of vector point straight at center of earth
double l2 = sqrtf(x2*x2 + y2*y2 + z2*z2);
std::cout << "velocity correct " << l1
<< ", dx " << (dx);
// << ", radius " << EARTH_RADIUS_METERS
// << ", coslat " << cos(state.latitude)*cos(state.longitude);
//<< ", dy " << (y1)
//<< ", dz " << (z1);
/*
double dotprod = (
(dx/l1 * (-x2)/l2) +
(dy/l1 * (-y2)/l2) +
(dz/l1 * (-z2)/l2) );
state.pitch = acos(dotprod)/M_PI -0.5;
*/
}
j++;
state_ints.target_long= float_to_fixed(target_longitude);
state_ints.target_lat = float_to_fixed(target_latitude);
state_ints.target_alt = float_to_fixed(target_altitude);
state_ints.p = float_to_fixed(buf.p);
state_ints.q = float_to_fixed(buf.q);
state_ints.r = float_to_fixed(buf.r);
state_ints.A_X_pilot = float_to_fixed(buf.A_X_pilot);
state_ints.A_Y_pilot = float_to_fixed(buf.A_Y_pilot);
state_ints.A_Z_pilot = float_to_fixed(buf.A_Z_pilot);
#endif
if (received < 0) {
perror("recvfrom");
// } else if (received != echolen) {
// std::cerr << "wrong sized packet " << received << std::endl;
} else {
/// get the ctrls that are properly filled out with environmental
/// data, so we don't clobber that when we modify the flap positions
received = recvfrom(sock2, buffer_ctrl, sizeof(bufctrl), 0,
(struct sockaddr *) &ctrlinclient,
&echolen);
FGProps2NetCtrls(&bufctrl);
float dt =1.0/10.0;
autopilot(state, old_state,dt);
bufctrl.elevator = FPFIX(state.elevator);
bufctrl.aileron = FPFIX(state.aileron);
bufctrl.rudder = FPFIX(state.rudder);
//autopilot_ints(state_ints, old_state_ints, buf, bufctrl);
//state_fixed_to_float(state_ints,state);
/// output to file and stdout
{
/// save telemetry to file
telem <<
buf.longitude << "," << buf.latitude << "," << buf.altitude*M2FT << "," <<
state.p << "," << state.q << "," << state.r << "," <<
bufctrl.elevator << "," << bufctrl.rudder << "," << bufctrl.aileron << "," <<
state.dq << "," << state.iq << "," <<
state.error_heading << "," << state.derror_heading << "," << state.ierror_heading << "," <<
state.error_pitch << "," << state.dpitch << "," << state.ipitch << "," <<
state.tpitch << "," << state.speed << "," <<
state.tdx << "," << state.tdy << "," <<
bufctrl.wind_speed_kt << "," << bufctrl.wind_dir_deg << "," << bufctrl.press_inhg << "," <<
state.dr << "," << state.ir << "," << state.pitch <<
state.A_X_pilot << "," << state.A_Y_pilot << "," << state.A_Z_pilot << "," <<
state.longitude << "," << state.latitude << "," << state.altitude << "," <<
std::endl;
/// output some of the state to stdout every few seconds
static int i = 0;
i++;
std::cout.precision(2);
//if ((state.longitude != old_state.longitude ) || (state.latitude != old_state.latitude)) {
if (i%30==0) {
std::cout <<
", hor dist=" << sqrtf(state.tdist*state.tdist - (state.altitude-target_altitude)*(state.altitude-target_altitude)) <<
//", agl=" << buf.agl << ", alt=" << state.altitude << ", vel=" << state.speed <<
// ", fdm v= " << sqrtf(buf.v_north*buf.v_north + buf.v_east*buf.v_east + buf.v_down*buf.v_down)*0.3048 <<
// " tdx=" << state.tdx << ", tdy=" << state.tdy <<
// " heading= " << heading << " target heading= " << theading <<
", err_head= " << state.error_heading << ", rud=" << bufctrl.rudder <<
// ", lat= " << state.latitude << ", long= " << state.longitude << ", alt= " << state.altitude <<
", pitch= " << state.pitch << ", tpitch= " << state.tpitch << ", elev=" << bufctrl.elevator <<
// ", p=" << state.p <<
// ", q=" << state.q << ", elev=" << bufctrl.elevator <<
", r=" << state.r << ", ail= " << bufctrl.aileron <<
", ax=" << state.A_X_pilot << ", ay=" << state.A_Y_pilot << ", az=" << state.A_Z_pilot <<
", orient=" << state.acc_orientation <<
std::endl;
}
}
fwrite((void*)&state, 1, sizeof(state), telem_file);
//std::cout << old_state.altitude << " " << state.altitude << std::endl;
memcpy(&old_state, &state, sizeof(state));
//memcpy(&old_state_ints, &state_ints, sizeof(state_ints));
////////////////////////////////////////////////////////////////////////
received = sendto(sock, buffer_ctrl, sizeof(bufctrl), 0,
(struct sockaddr *) &ctrlclient, sizeof(ctrlclient));
if (received <0) {
perror("sendto");
}
time += 0.1;
}
usleep(3000);
}
fclose(telem_file);
return 0;
}
