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; }