Exemple #1
0
bool event_log( const char *hdr, const char *msg ) {
    fprintf( fevent, "%.3f %s %s\n", get_Time(), hdr, msg );
    fflush( fevent );

    return true;
}
Exemple #2
0
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();
}