bool event_log( const char *hdr, const char *msg ) {
fprintf( fevent, "%.3f %s %s\n", get_Time(), hdr, msg );
fflush( fevent );
return true;
}
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();
}
