void control_init() {
// initialize the autopilot class and build the structures from the
// configuration file values
ap.init();
ap.build();
// initialize the flight control output property nodes
aileron_out_node = fgGetNode("/controls/flight/aileron", true);
elevator_out_node = fgGetNode("/controls/flight/elevator", true);
throttle_out_node = fgGetNode("/engines/engine[0]/throttle", true);
rudder_out_node = fgGetNode("/controls/flight/rudder", true);
ap_target = fgGetNode("/autopilot/settings/target-roll-deg", true);
elevon_mix = fgGetNode("/config/autopilot/elevon-mixing", true);
}
void control_init() {
// initialize the autopilot class and build the structures from the
// configuration file values
bind_properties();
ap.init();
ap.build();
// set default autopilot modes
// ap_heading_mode_node->setStringValue("route");
// ap_roll_mode_node->setStringValue("aileron");
// ap_yaw_mode_node->setStringValue("turn-coord");
// ap_altitude_mode_node->setStringValue("pitch");
// ap_speed_mode_node->setStringValue("throttle");
// ap_pitch_mode_node->setStringValue("elevator");
if ( display_on ) {
printf("Autopilot initialized\n");
}
}
void control_update(short flight_mode)
{
// make a quick exit if we are disabled
if ( !autopilot_active ) {
return;
}
// reset the autopilot if requested
if ( autopilot_reinit ) {
control_reset();
autopilot_reinit = false;
}
// optional: use channel #6 to change the autopilot target value
// double min_value = -35.0;
// double max_value = 35.0;
// double tgt_value = (max_value - min_value) *
// ((double)servo_in.chn[5] / 65535.0) + min_value;
// ap_target->setFloatValue( tgt_value );
// update the autopilot stages
ap.update( 0.04 ); // dt = 1/25
/* printf("%.2f %.2f\n", aileron_out_node->getFloatValue(),
elevator_out_node->getFloatValue()); */
static SGPropertyNode *vert_speed_fps
= fgGetNode("/velocities/vertical-speed-fps", true);
static SGPropertyNode *true_alt
= fgGetNode("/position/altitude-ft", true);
/* printf("%.1f %.2f %.2f\n",
true_alt->getFloatValue(),
vert_speed_fps->getFloatValue(),
elevator_out_node->getFloatValue()); */
// initialize the servo command array to central values so we don't
// inherit junk
for ( int i = 0; i < 8; ++i ) {
servo_out.chn[i] = 32768;
}
if ( elevon_mix->getBoolValue() ) {
// elevon mixing mode
//aileron
servo_out.chn[0] = 32768
+ (int16_t)(aileron_out_node->getFloatValue() * 32768)
+ (int16_t)(elevator_out_node->getFloatValue() * 32768);
//elevator
servo_out.chn[1] = 32768
+ (int16_t)(aileron_out_node->getFloatValue() * 32768)
- (int16_t)(elevator_out_node->getFloatValue() * 32768);
} else {
// conventional airframe mode
//aileron
servo_out.chn[0] = 32768
+ (int16_t)(aileron_out_node->getFloatValue() * 32768);
//elevator
servo_out.chn[1] = 32768
+ (int16_t)(elevator_out_node->getFloatValue() * 32768);
}
//throttle
servo_out.chn[2] = 32768
+ (uint16_t)(throttle_out_node->getFloatValue() * 32768);
// rudder
servo_out.chn[3] = 32768
+ (int16_t)(rudder_out_node->getFloatValue() * 32768);
// time stamp the packet for logging
servo_out.time = get_Time();
// send commanded servo positions to the MNAV
send_short_servo_cmd();
}
void control_update(double dt)
{
static bool fcs_enabled = false;
if ( ap_master_switch_node->getBoolValue() ) {
if ( !fcs_enabled ) {
if ( strcmp(fcs_mode_node->getStringValue(), "route") == 0 ) {
// fcs is just activated, set lock modes for "route"
heading_lock_node->setStringValue( "route" );
roll_lock_node->setStringValue( "aileron" );
yaw_lock_node->setStringValue( "turn-coord" );
altitude_lock_node->setStringValue( "pitch" );
speed_lock_node->setStringValue( "throttle" );
pitch_lock_node->setStringValue( "elevator" );
pointing_lock_node->setStringValue( "on" );
lookat_mode_node->setStringValue( "ned-vector" );
ned_n_node->setFloatValue( 0.0 );
ned_e_node->setFloatValue( 0.0 );
ned_d_node->setFloatValue( 1.0 );
float target_speed_kt = target_speed_node->getFloatValue();
float initial_speed_kt = initial_speed_node->getFloatValue();
if ( target_speed_kt < 0.1 ) {
target_speed_node->setFloatValue( initial_speed_kt );
}
fcs_enabled = true;
} else if ( strcmp(fcs_mode_node->getStringValue(), "cas") == 0 ) {
// fcs is just activated, set lock modes for "cas"
heading_lock_node->setStringValue( "" );
roll_lock_node->setStringValue( "aileron" );
yaw_lock_node->setStringValue( "" );
altitude_lock_node->setStringValue( "" );
speed_lock_node->setStringValue( "" );
pitch_lock_node->setStringValue( "elevator" );
pointing_lock_node->setStringValue( "on" );
lookat_mode_node->setStringValue( "ned-vector" );
ned_n_node->setFloatValue( 0.0 );
ned_e_node->setFloatValue( 0.0 );
ned_d_node->setFloatValue( 1.0 );
float target_roll_deg = roll_deg_node->getFloatValue();
if ( target_roll_deg > 45.0 ) { target_roll_deg = 45.0; }
if ( target_roll_deg < -45.0 ) { target_roll_deg = -45.0; }
target_roll_deg_node->setFloatValue( target_roll_deg );
float target_pitch_base_deg = pitch_deg_node->getFloatValue();
if ( target_pitch_base_deg > 15.0 ) {
target_pitch_base_deg = 15.0;
}
if ( target_pitch_base_deg < -15.0 ) {
target_pitch_base_deg = -15.0;
}
target_pitch_base_deg_node->setFloatValue( target_pitch_base_deg );
float target_speed_kt = target_speed_node->getFloatValue();
float initial_speed_kt = initial_speed_node->getFloatValue();
if ( target_speed_kt < 0.1 ) {
target_speed_node->setFloatValue( initial_speed_kt );
}
fcs_enabled = true;
}
}
} else {
if ( fcs_enabled ) {
// autopilot is just de-activated, clear lock modes
heading_lock_node->setStringValue( "" );
roll_lock_node->setStringValue( "" );
yaw_lock_node->setStringValue( "" );
altitude_lock_node->setStringValue( "" );
speed_lock_node->setStringValue( "" );
pitch_lock_node->setStringValue( "" );
pointing_lock_node->setStringValue( "" );
fcs_enabled = false;
}
}
if ( fcs_enabled ) {
// update the autopilot stages
ap.update( dt );
}
}
void control_reinit() {
// reread autopilot configuration from the property tree and reset
// all stages (i.e. real time gain tuning)
ap.reinit();
}