Esempio n. 1
0
int
sc_set_graphics_mode(scr_stat *scp, struct tty *tp, int mode)
{
#ifdef SC_NO_MODE_CHANGE
    return ENODEV;
#else
    video_info_t info;
    int error;
    int s;

    if (vidd_get_info(scp->sc->adp, mode, &info))
	return ENODEV;

    /* stop screen saver, etc */
    s = spltty();
    if ((error = sc_clean_up(scp))) {
	splx(s);
	return error;
    }

    if (sc_render_match(scp, scp->sc->adp->va_name, GRAPHICS_MODE) == NULL) {
	splx(s);
	return ENODEV;
    }

    /* set up scp */
    scp->status |= (UNKNOWN_MODE | GRAPHICS_MODE | MOUSE_HIDDEN);
    scp->status &= ~(PIXEL_MODE | MOUSE_VISIBLE);
    scp->mode = mode;
    /*
     * Don't change xsize and ysize; preserve the previous vty
     * and history buffers.
     */
    scp->xoff = 0;
    scp->yoff = 0;
    scp->xpixel = info.vi_width;
    scp->ypixel = info.vi_height;
    scp->font = NULL;
    scp->font_size = 0;
#ifndef SC_NO_SYSMOUSE
    /* move the mouse cursor at the center of the screen */
    sc_mouse_move(scp, scp->xpixel / 2, scp->ypixel / 2);
#endif
    sc_init_emulator(scp, NULL);
    splx(s);

    if (scp == scp->sc->cur_scp)
	set_mode(scp);
    /* clear_graphics();*/
    scp->status &= ~UNKNOWN_MODE;

    if (tp == NULL)
	return 0;
    if (tp->t_winsize.ws_xpixel != scp->xpixel
	|| tp->t_winsize.ws_ypixel != scp->ypixel) {
	tp->t_winsize.ws_xpixel = scp->xpixel;
	tp->t_winsize.ws_ypixel = scp->ypixel;

	tty_signal_pgrp(tp, SIGWINCH);
    }

    return 0;
#endif /* SC_NO_MODE_CHANGE */
}
Esempio n. 2
0
// do_aux_switch_function - implement the function invoked by the ch7 or ch8 switch
void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
{

    switch(ch_function) {
        case AUXSW_FLIP:
            // flip if switch is on, positive throttle and we're actually flying
            if(ch_flag == AUX_SWITCH_HIGH) {
                set_mode(FLIP, MODE_REASON_TX_COMMAND);
            }
            break;

        case AUXSW_SIMPLE_MODE:
            // low = simple mode off, middle or high position turns simple mode on
            set_simple_mode(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
            break;

        case AUXSW_SUPERSIMPLE_MODE:
            // low = simple mode off, middle = simple mode, high = super simple mode
            set_simple_mode(ch_flag);
            break;

        case AUXSW_RTL:
            if (ch_flag == AUX_SWITCH_HIGH) {
                // engage RTL (if not possible we remain in current flight mode)
                set_mode(RTL, MODE_REASON_TX_COMMAND);
            }else{
                // return to flight mode switch's flight mode if we are currently in RTL
                if (control_mode == RTL) {
                    reset_control_switch();
                }
            }
            break;

        case AUXSW_SAVE_TRIM:
            if ((ch_flag == AUX_SWITCH_HIGH) && (control_mode <= ACRO) && (channel_throttle->get_control_in() == 0)) {
                save_trim();
            }
            break;

        case AUXSW_SAVE_WP:
            // save waypoint when switch is brought high
            if (ch_flag == AUX_SWITCH_HIGH) {

                // do not allow saving new waypoints while we're in auto or disarmed
                if(control_mode == AUTO || !motors.armed()) {
                    return;
                }

                // do not allow saving the first waypoint with zero throttle
                if((mission.num_commands() == 0) && (channel_throttle->get_control_in() == 0)){
                    return;
                }

                // create new mission command
                AP_Mission::Mission_Command cmd  = {};

                // if the mission is empty save a takeoff command
                if(mission.num_commands() == 0) {
                    // set our location ID to 16, MAV_CMD_NAV_WAYPOINT
                    cmd.id = MAV_CMD_NAV_TAKEOFF;
                    cmd.content.location.options = 0;
                    cmd.p1 = 0;
                    cmd.content.location.lat = 0;
                    cmd.content.location.lng = 0;
                    cmd.content.location.alt = MAX(current_loc.alt,100);

                    // use the current altitude for the target alt for takeoff.
                    // only altitude will matter to the AP mission script for takeoff.
                    if(mission.add_cmd(cmd)) {
                        // log event
                        Log_Write_Event(DATA_SAVEWP_ADD_WP);
                    }
                }

                // set new waypoint to current location
                cmd.content.location = current_loc;

                // if throttle is above zero, create waypoint command
                if(channel_throttle->get_control_in() > 0) {
                    cmd.id = MAV_CMD_NAV_WAYPOINT;
                }else{
                    // with zero throttle, create LAND command
                    cmd.id = MAV_CMD_NAV_LAND;
                }

                // save command
                if(mission.add_cmd(cmd)) {
                    // log event
                    Log_Write_Event(DATA_SAVEWP_ADD_WP);
                }
            }
            break;

#if CAMERA == ENABLED
        case AUXSW_CAMERA_TRIGGER:
            if (ch_flag == AUX_SWITCH_HIGH) {
                do_take_picture();
            }
            break;
#endif

        case AUXSW_RANGEFINDER:
            // enable or disable the rangefinder
#if RANGEFINDER_ENABLED == ENABLED
            if ((ch_flag == AUX_SWITCH_HIGH) && (rangefinder.num_sensors() >= 1)) {
                rangefinder_state.enabled = true;
            }else{
                rangefinder_state.enabled = false;
            }
#endif
            break;

#if AC_FENCE == ENABLED
        case AUXSW_FENCE:
            // enable or disable the fence
            if (ch_flag == AUX_SWITCH_HIGH) {
                fence.enable(true);
                Log_Write_Event(DATA_FENCE_ENABLE);
            }else{
                fence.enable(false);
                Log_Write_Event(DATA_FENCE_DISABLE);
            }
            break;
#endif

        case AUXSW_ACRO_TRAINER:
            switch(ch_flag) {
                case AUX_SWITCH_LOW:
                    g.acro_trainer = ACRO_TRAINER_DISABLED;
                    Log_Write_Event(DATA_ACRO_TRAINER_DISABLED);
                    break;
                case AUX_SWITCH_MIDDLE:
                    g.acro_trainer = ACRO_TRAINER_LEVELING;
                    Log_Write_Event(DATA_ACRO_TRAINER_LEVELING);
                    break;
                case AUX_SWITCH_HIGH:
                    g.acro_trainer = ACRO_TRAINER_LIMITED;
                    Log_Write_Event(DATA_ACRO_TRAINER_LIMITED);
                    break;
            }
            break;
#if EPM_ENABLED == ENABLED
        case AUXSW_EPM:
            switch(ch_flag) {
                case AUX_SWITCH_LOW:
                    epm.release();
                    Log_Write_Event(DATA_EPM_RELEASE);
                    break;
                case AUX_SWITCH_HIGH:
                    epm.grab();
                    Log_Write_Event(DATA_EPM_GRAB);
                    break;
            }
            break;
#endif
#if SPRAYER == ENABLED
        case AUXSW_SPRAYER:
            sprayer.enable(ch_flag == AUX_SWITCH_HIGH);
            // if we are disarmed the pilot must want to test the pump
            sprayer.test_pump((ch_flag == AUX_SWITCH_HIGH) && !motors.armed());
            break;
#endif

        case AUXSW_AUTO:
            if (ch_flag == AUX_SWITCH_HIGH) {
                set_mode(AUTO, MODE_REASON_TX_COMMAND);
            }else{
                // return to flight mode switch's flight mode if we are currently in AUTO
                if (control_mode == AUTO) {
                    reset_control_switch();
                }
            }
            break;

#if AUTOTUNE_ENABLED == ENABLED
        case AUXSW_AUTOTUNE:
            // turn on auto tuner
            switch(ch_flag) {
                case AUX_SWITCH_LOW:
                case AUX_SWITCH_MIDDLE:
                    // restore flight mode based on flight mode switch position
                    if (control_mode == AUTOTUNE) {
                        reset_control_switch();
                    }
                    break;
                case AUX_SWITCH_HIGH:
                    // start an autotuning session
                    set_mode(AUTOTUNE, MODE_REASON_TX_COMMAND);
                    break;
            }
            break;
#endif

        case AUXSW_LAND:
            if (ch_flag == AUX_SWITCH_HIGH) {
                set_mode(LAND, MODE_REASON_TX_COMMAND);
            }else{
                // return to flight mode switch's flight mode if we are currently in LAND
                if (control_mode == LAND) {
                    reset_control_switch();
                }
            }
            break;

#if PARACHUTE == ENABLED
        case AUXSW_PARACHUTE_ENABLE:
            // Parachute enable/disable
            parachute.enabled(ch_flag == AUX_SWITCH_HIGH);
            break;

        case AUXSW_PARACHUTE_RELEASE:
            if (ch_flag == AUX_SWITCH_HIGH) {
                parachute_manual_release();
            }
            break;

        case AUXSW_PARACHUTE_3POS:
            // Parachute disable, enable, release with 3 position switch
            switch (ch_flag) {
                case AUX_SWITCH_LOW:
                    parachute.enabled(false);
                    Log_Write_Event(DATA_PARACHUTE_DISABLED);
                    break;
                case AUX_SWITCH_MIDDLE:
                    parachute.enabled(true);
                    Log_Write_Event(DATA_PARACHUTE_ENABLED);
                    break;
                case AUX_SWITCH_HIGH:
                    parachute.enabled(true);
                    parachute_manual_release();
                    break;
            }
            break;
#endif

        case AUXSW_MISSION_RESET:
            if (ch_flag == AUX_SWITCH_HIGH) {
                mission.reset();
            }
            break;

        case AUXSW_ATTCON_FEEDFWD:
            // enable or disable feed forward
            attitude_control.bf_feedforward(ch_flag == AUX_SWITCH_HIGH);
            break;

        case AUXSW_ATTCON_ACCEL_LIM:
            // enable or disable accel limiting by restoring defaults
            attitude_control.accel_limiting(ch_flag == AUX_SWITCH_HIGH);
            break;
        
#if MOUNT == ENABLE
        case AUXSW_RETRACT_MOUNT:
            switch (ch_flag) {
                case AUX_SWITCH_HIGH:
                    camera_mount.set_mode(MAV_MOUNT_MODE_RETRACT);
                    break;
                case AUX_SWITCH_LOW:
                    camera_mount.set_mode_to_default();
                    break;
            }
            break;
#endif

        case AUXSW_RELAY:
            ServoRelayEvents.do_set_relay(0, ch_flag == AUX_SWITCH_HIGH);
            break;

        case AUXSW_RELAY2:
            ServoRelayEvents.do_set_relay(1, ch_flag == AUX_SWITCH_HIGH);
            break;

        case AUXSW_RELAY3:
            ServoRelayEvents.do_set_relay(2, ch_flag == AUX_SWITCH_HIGH);
            break;

	   case AUXSW_RELAY4:
            ServoRelayEvents.do_set_relay(3, ch_flag == AUX_SWITCH_HIGH);
            break;

		case AUXSW_LANDING_GEAR:
            switch (ch_flag) {
                case AUX_SWITCH_LOW:
                    landinggear.set_cmd_mode(LandingGear_Deploy);
                    break;
                case AUX_SWITCH_MIDDLE:
                    landinggear.set_cmd_mode(LandingGear_Auto);
                    break;
                case AUX_SWITCH_HIGH:
                    landinggear.set_cmd_mode(LandingGear_Retract);
                    break;
            }
            break;

        case AUXSW_LOST_COPTER_SOUND:
            switch (ch_flag) {
                case AUX_SWITCH_HIGH:
                    AP_Notify::flags.vehicle_lost = true;
                    break;
                case AUX_SWITCH_LOW:
                    AP_Notify::flags.vehicle_lost = false;
                    break;
            }
            break;

        case AUXSW_MOTOR_ESTOP:
            // Turn on Emergency Stop logic when channel is high
            set_motor_emergency_stop(ch_flag == AUX_SWITCH_HIGH);
            break;

        case AUXSW_MOTOR_INTERLOCK:
            // Turn on when above LOW, because channel will also be used for speed
            // control signal in tradheli
            ap.motor_interlock_switch = (ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
            break;

        case AUXSW_BRAKE:
            // brake flight mode
            if (ch_flag == AUX_SWITCH_HIGH) {
                set_mode(BRAKE, MODE_REASON_TX_COMMAND);
            }else{
                // return to flight mode switch's flight mode if we are currently in BRAKE
                if (control_mode == BRAKE) {
                    reset_control_switch();
                }
            }
            break;

        case AUXSW_THROW:
            // throw flight mode
            if (ch_flag == AUX_SWITCH_HIGH) {
                set_mode(THROW, MODE_REASON_TX_COMMAND);
            } else {
                // return to flight mode switch's flight mode if we are currently in throw mode
                if (control_mode == THROW) {
                    reset_control_switch();
                }
            }
            break;

        case AUXSW_AVOID_ADSB:
            // enable or disable AP_Avoidance
            if (ch_flag == AUX_SWITCH_HIGH) {
                avoidance_adsb.enable();
                Log_Write_Event(DATA_AVOIDANCE_ADSB_ENABLE);
            }else{
                avoidance_adsb.disable();
                Log_Write_Event(DATA_AVOIDANCE_ADSB_DISABLE);
            }
            break;
    }
}
Esempio n. 3
0
void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
{
    // Act based on the function assigned to this button
    switch (get_button(button)->function(shift)) {
    case JSButton::button_function_t::k_arm_toggle:
        if (motors.armed()) {
            init_disarm_motors();
        } else {
            init_arm_motors(AP_Arming::ArmingMethod::MAVLINK);
        }
        break;
    case JSButton::button_function_t::k_arm:
        init_arm_motors(AP_Arming::ArmingMethod::MAVLINK);
        break;
    case JSButton::button_function_t::k_disarm:
        init_disarm_motors();
        break;

    case JSButton::button_function_t::k_mode_manual:
        set_mode(MANUAL, MODE_REASON_TX_COMMAND);
        break;
    case JSButton::button_function_t::k_mode_stabilize:
        set_mode(STABILIZE, MODE_REASON_TX_COMMAND);
        break;
    case JSButton::button_function_t::k_mode_depth_hold:
        set_mode(ALT_HOLD, MODE_REASON_TX_COMMAND);
        break;
    case JSButton::button_function_t::k_mode_auto:
        set_mode(AUTO, MODE_REASON_TX_COMMAND);
        break;
    case JSButton::button_function_t::k_mode_guided:
        set_mode(GUIDED, MODE_REASON_TX_COMMAND);
        break;
    case JSButton::button_function_t::k_mode_circle:
        set_mode(CIRCLE, MODE_REASON_TX_COMMAND);
        break;
    case JSButton::button_function_t::k_mode_acro:
        set_mode(ACRO, MODE_REASON_TX_COMMAND);
        break;
    case JSButton::button_function_t::k_mode_poshold:
        set_mode(POSHOLD, MODE_REASON_TX_COMMAND);
        break;

    case JSButton::button_function_t::k_mount_center:
#if MOUNT == ENABLED
        camera_mount.set_angle_targets(0, 0, 0);
        // for some reason the call to set_angle_targets changes the mode to mavlink targeting!
        camera_mount.set_mode(MAV_MOUNT_MODE_RC_TARGETING);
#endif
        break;
    case JSButton::button_function_t::k_mount_tilt_up:
        cam_tilt = 1900;
        break;
    case JSButton::button_function_t::k_mount_tilt_down:
        cam_tilt = 1100;
        break;
    case JSButton::button_function_t::k_camera_trigger:
        break;
    case JSButton::button_function_t::k_camera_source_toggle:
        if (!held) {
            static bool video_toggle = false;
            video_toggle = !video_toggle;
            if (video_toggle) {
                video_switch = 1900;
                gcs().send_text(MAV_SEVERITY_INFO,"Video Toggle: Source 2");
            } else {
                video_switch = 1100;
                gcs().send_text(MAV_SEVERITY_INFO,"Video Toggle: Source 1");
            }
        }
        break;
    case JSButton::button_function_t::k_mount_pan_right:
        cam_pan = 1900;
        break;
    case JSButton::button_function_t::k_mount_pan_left:
        cam_pan = 1100;
        break;
    case JSButton::button_function_t::k_lights1_cycle:
        if (!held) {
            static bool increasing = true;
            RC_Channel* chan = RC_Channels::rc_channel(8);
            uint16_t min = chan->get_radio_min();
            uint16_t max = chan->get_radio_max();
            uint16_t step = (max - min) / g.lights_steps;
            if (increasing) {
                lights1 = constrain_float(lights1 + step, min, max);
            } else {
                lights1 = constrain_float(lights1 - step, min, max);
            }
            if (lights1 >= max || lights1 <= min) {
                increasing = !increasing;
            }
        }
        break;
    case JSButton::button_function_t::k_lights1_brighter:
        if (!held) {
            RC_Channel* chan = RC_Channels::rc_channel(8);
            uint16_t min = chan->get_radio_min();
            uint16_t max = chan->get_radio_max();
            uint16_t step = (max - min) / g.lights_steps;
            lights1 = constrain_float(lights1 + step, min, max);
        }
        break;
    case JSButton::button_function_t::k_lights1_dimmer:
        if (!held) {
            RC_Channel* chan = RC_Channels::rc_channel(8);
            uint16_t min = chan->get_radio_min();
            uint16_t max = chan->get_radio_max();
            uint16_t step = (max - min) / g.lights_steps;
            lights1 = constrain_float(lights1 - step, min, max);
        }
        break;
    case JSButton::button_function_t::k_lights2_cycle:
        if (!held) {
            static bool increasing = true;
            RC_Channel* chan = RC_Channels::rc_channel(9);
            uint16_t min = chan->get_radio_min();
            uint16_t max = chan->get_radio_max();
            uint16_t step = (max - min) / g.lights_steps;
            if (increasing) {
                lights2 = constrain_float(lights2 + step, min, max);
            } else {
                lights2 = constrain_float(lights2 - step, min, max);
            }
            if (lights2 >= max || lights2 <= min) {
                increasing = !increasing;
            }
        }
        break;
    case JSButton::button_function_t::k_lights2_brighter:
        if (!held) {
            RC_Channel* chan = RC_Channels::rc_channel(9);
            uint16_t min = chan->get_radio_min();
            uint16_t max = chan->get_radio_max();
            uint16_t step = (max - min) / g.lights_steps;
            lights2 = constrain_float(lights2 + step, min, max);
        }
        break;
    case JSButton::button_function_t::k_lights2_dimmer:
        if (!held) {
            RC_Channel* chan = RC_Channels::rc_channel(9);
            uint16_t min = chan->get_radio_min();
            uint16_t max = chan->get_radio_max();
            uint16_t step = (max - min) / g.lights_steps;
            lights2 = constrain_float(lights2 - step, min, max);
        }
        break;
    case JSButton::button_function_t::k_gain_toggle:
        if (!held) {
            static bool lowGain = false;
            lowGain = !lowGain;
            if (lowGain) {
                gain = 0.5f;
            } else {
                gain = 1.0f;
            }
            gcs().send_text(MAV_SEVERITY_INFO,"#Gain: %2.0f%%",(double)gain*100);
        }
        break;
    case JSButton::button_function_t::k_gain_inc:
        if (!held) {
            // check that our gain parameters are in correct range, update in eeprom and notify gcs if needed
            g.minGain.set_and_save(constrain_float(g.minGain, 0.10, 0.80));
            g.maxGain.set_and_save(constrain_float(g.maxGain, g.minGain, 1.0));
            g.numGainSettings.set_and_save(constrain_int16(g.numGainSettings, 1, 10));

            if (g.numGainSettings == 1) {
                gain = constrain_float(g.gain_default, g.minGain, g.maxGain);
            } else {
                gain = constrain_float(gain + (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain);
            }

            gcs().send_text(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",(double)gain*100);
        }
        break;
    case JSButton::button_function_t::k_gain_dec:
        if (!held) {
            // check that our gain parameters are in correct range, update in eeprom and notify gcs if needed
            g.minGain.set_and_save(constrain_float(g.minGain, 0.10, 0.80));
            g.maxGain.set_and_save(constrain_float(g.maxGain, g.minGain, 1.0));
            g.numGainSettings.set_and_save(constrain_int16(g.numGainSettings, 1, 10));

            if (g.numGainSettings == 1) {
                gain = constrain_float(g.gain_default, g.minGain, g.maxGain);
            } else {
                gain = constrain_float(gain - (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain);
            }

            gcs().send_text(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",(double)gain*100);
        }
        break;
    case JSButton::button_function_t::k_trim_roll_inc:
        rollTrim = constrain_float(rollTrim+10,-200,200);
        break;
    case JSButton::button_function_t::k_trim_roll_dec:
        rollTrim = constrain_float(rollTrim-10,-200,200);
        break;
    case JSButton::button_function_t::k_trim_pitch_inc:
        pitchTrim = constrain_float(pitchTrim+10,-200,200);
        break;
    case JSButton::button_function_t::k_trim_pitch_dec:
        pitchTrim = constrain_float(pitchTrim-10,-200,200);
        break;
    case JSButton::button_function_t::k_input_hold_set:
        if(!motors.armed()) {
            break;
        }
        if (!held) {
            zTrim = abs(z_last-500) > 50 ? z_last-500 : 0;
            xTrim = abs(x_last) > 50 ? x_last : 0;
            yTrim = abs(y_last) > 50 ? y_last : 0;
            bool input_hold_engaged_last = input_hold_engaged;
            input_hold_engaged = zTrim || xTrim || yTrim;
            if (input_hold_engaged) {
                gcs().send_text(MAV_SEVERITY_INFO,"#Input Hold Set");
            } else if (input_hold_engaged_last) {
                gcs().send_text(MAV_SEVERITY_INFO,"#Input Hold Disabled");
            }
        }
        break;
    case JSButton::button_function_t::k_relay_1_on:
        relay.on(0);
        break;
    case JSButton::button_function_t::k_relay_1_off:
        relay.off(0);
        break;
    case JSButton::button_function_t::k_relay_1_toggle:
        if (!held) {
            relay.toggle(0);
        }
        break;
    case JSButton::button_function_t::k_relay_1_momentary:
        if (!held) {
            relay.on(0);
        }
        break;
    case JSButton::button_function_t::k_relay_2_on:
        relay.on(1);
        break;
    case JSButton::button_function_t::k_relay_2_off:
        relay.off(1);
        break;
    case JSButton::button_function_t::k_relay_2_toggle:
        if (!held) {
            relay.toggle(1);
        }
        break;
    case JSButton::button_function_t::k_relay_2_momentary:
        if (!held) {
            relay.on(1);
        }
        break;
    case JSButton::button_function_t::k_relay_3_on:
        relay.on(2);
        break;
    case JSButton::button_function_t::k_relay_3_off:
        relay.off(2);
        break;
    case JSButton::button_function_t::k_relay_3_toggle:
        if (!held) {
            relay.toggle(2);
        }
        break;
    case JSButton::button_function_t::k_relay_3_momentary:
        if (!held) {
            relay.on(2);
        }
        break;
    case JSButton::button_function_t::k_relay_4_on:
        relay.on(3);
        break;
    case JSButton::button_function_t::k_relay_4_off:
        relay.off(3);
        break;
    case JSButton::button_function_t::k_relay_4_toggle:
        if (!held) {
            relay.toggle(3);
        }
        break;
    case JSButton::button_function_t::k_relay_4_momentary:
        if (!held) {
            relay.on(3);
        }
        break;

    ////////////////////////////////////////////////
    // Servo functions
    // TODO: initialize
    case JSButton::button_function_t::k_servo_1_inc:
    {
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
        uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_1 - 1); // 0-indexed
        pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max());
        ServoRelayEvents.do_set_servo(SERVO_CHAN_1, pwm_out); // 1-indexed
    }
        break;
    case JSButton::button_function_t::k_servo_1_dec:
    {
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
        uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_1 - 1); // 0-indexed
        pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max());
        ServoRelayEvents.do_set_servo(SERVO_CHAN_1, pwm_out); // 1-indexed
    }
        break;
    case JSButton::button_function_t::k_servo_1_min:
    case JSButton::button_function_t::k_servo_1_min_momentary:
    {
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
        ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_min()); // 1-indexed
    }
        break;
    case JSButton::button_function_t::k_servo_1_min_toggle:
        if(!held) {
            SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
            if(chan->get_output_pwm() != chan->get_output_min()) {
                ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_min()); // 1-indexed
            } else {
                ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_trim()); // 1-indexed
            }
        }
        break;
    case JSButton::button_function_t::k_servo_1_max:
    case JSButton::button_function_t::k_servo_1_max_momentary:
    {
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
        ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_max()); // 1-indexed
    }
        break;
    case JSButton::button_function_t::k_servo_1_max_toggle:
        if(!held) {
            SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
            if(chan->get_output_pwm() != chan->get_output_max()) {
                ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_max()); // 1-indexed
            } else {
                ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_trim()); // 1-indexed
            }
        }
        break;
    case JSButton::button_function_t::k_servo_1_center:
    {
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
        ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_trim()); // 1-indexed
    }
        break;

    case JSButton::button_function_t::k_servo_2_inc:
    {
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
        uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_2 - 1); // 0-indexed
        pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max());
        ServoRelayEvents.do_set_servo(SERVO_CHAN_2, pwm_out); // 1-indexed
    }
        break;
    case JSButton::button_function_t::k_servo_2_dec:
    {
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
        uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_2 - 1); // 0-indexed
        pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max());
        ServoRelayEvents.do_set_servo(SERVO_CHAN_2, pwm_out); // 1-indexed
    }
        break;
    case JSButton::button_function_t::k_servo_2_min:
    case JSButton::button_function_t::k_servo_2_min_momentary:
    {
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
        ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_min()); // 1-indexed
    }
        break;
    case JSButton::button_function_t::k_servo_2_max:
    case JSButton::button_function_t::k_servo_2_max_momentary:
    {
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
        ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_max()); // 1-indexed
    }
        break;
    case JSButton::button_function_t::k_servo_2_center:
    {
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
        ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_trim()); // 1-indexed
    }
        break;

    case JSButton::button_function_t::k_servo_3_inc:
    {
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
        uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_3 - 1); // 0-indexed
        pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max());
        ServoRelayEvents.do_set_servo(SERVO_CHAN_3, pwm_out); // 1-indexed
    }
        break;
    case JSButton::button_function_t::k_servo_3_dec:
    {
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
        uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_3 - 1); // 0-indexed
        pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max());
        ServoRelayEvents.do_set_servo(SERVO_CHAN_3, pwm_out); // 1-indexed
    }
        break;
    case JSButton::button_function_t::k_servo_3_min:
    case JSButton::button_function_t::k_servo_3_min_momentary:
    {
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
        ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_min()); // 1-indexed
    }
        break;
    case JSButton::button_function_t::k_servo_3_max:
    case JSButton::button_function_t::k_servo_3_max_momentary:
    {
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
        ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_max()); // 1-indexed
    }
        break;
    case JSButton::button_function_t::k_servo_3_center:
    {
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
        ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_trim()); // 1-indexed
    }
        break;

    case JSButton::button_function_t::k_roll_pitch_toggle:
        if (!held) {
            roll_pitch_flag = !roll_pitch_flag;
        }
        break;

    case JSButton::button_function_t::k_custom_1:
        // Not implemented
        break;
    case JSButton::button_function_t::k_custom_2:
        // Not implemented
        break;
    case JSButton::button_function_t::k_custom_3:
        // Not implemented
        break;
    case JSButton::button_function_t::k_custom_4:
        // Not implemented
        break;
    case JSButton::button_function_t::k_custom_5:
        // Not implemented
        break;
    case JSButton::button_function_t::k_custom_6:
        // Not implemented
        break;
    }
}
bool IcoPackImageButton::on_button_press_event(GdkEventButton* event)
{
	bool res = false ;
	set_mode(3) ;
	return res ;
}
Esempio n. 5
0
int main(void)
{
	struct sigaction sigact;
	int r = 1;

	exit_sem = sem_open (SEM_NAME, O_CREAT, 0);
	if (!exit_sem) {
		fprintf(stderr, "failed to initialise semaphore error %d", errno);
		exit(1);
	}

	/* only using this semaphore in this process so go ahead and unlink it now */
	sem_unlink (SEM_NAME);

	r = libusb_init(NULL);
	if (r < 0) {
		fprintf(stderr, "failed to initialise libusb\n");
		exit(1);
	}

	r = find_dpfp_device();
	if (r < 0) {
		fprintf(stderr, "Could not find/open device\n");
		goto out;
	}

	r = libusb_claim_interface(devh, 0);
	if (r < 0) {
		fprintf(stderr, "usb_claim_interface error %d %s\n", r, strerror(-r));
		goto out;
	}
	printf("claimed interface\n");

	r = print_f0_data();
	if (r < 0)
		goto out_release;

	r = do_init();
	if (r < 0)
		goto out_deinit;

	/* async from here onwards */

	sigact.sa_handler = sighandler;
	sigemptyset(&sigact.sa_mask);
	sigact.sa_flags = 0;
	sigaction(SIGINT, &sigact, NULL);
	sigaction(SIGTERM, &sigact, NULL);
	sigaction(SIGQUIT, &sigact, NULL);

	r = pthread_create(&poll_thread, NULL, poll_thread_main, NULL);
	if (r)
		goto out_deinit;

	r = alloc_transfers();
	if (r < 0) {
		request_exit(1);
		pthread_join(poll_thread, NULL);
		goto out_deinit;
	}

	r = init_capture();
	if (r < 0) {
		request_exit(1);
		pthread_join(poll_thread, NULL);
		goto out_deinit;
	}

	while (!do_exit)
		sem_wait(exit_sem);

	printf("shutting down...\n");
	pthread_join(poll_thread, NULL);

	r = libusb_cancel_transfer(irq_transfer);
	if (r < 0) {
		request_exit(1);
		goto out_deinit;
	}

	r = libusb_cancel_transfer(img_transfer);
	if (r < 0) {
		request_exit(1);
		goto out_deinit;
	}

	while (img_transfer || irq_transfer)
		if (libusb_handle_events(NULL) < 0)
			break;

	if (do_exit == 1)
		r = 0;
	else
		r = 1;

out_deinit:
	libusb_free_transfer(img_transfer);
	libusb_free_transfer(irq_transfer);
	set_mode(0);
	set_hwstat(0x80);
out_release:
	libusb_release_interface(devh, 0);
out:
	libusb_close(devh);
	libusb_exit(NULL);
	return r >= 0 ? r : -r;
}
Esempio n. 6
0
static int reiser_copy(disk_t *disk_car, const partition_t *partition, dir_data_t *dir_data, const file_data_t *file)
{
  reiserfs_file_t *in;
  FILE *f_out;
  char *new_file;
  struct rfs_dir_struct *ls=(struct rfs_dir_struct*)dir_data->private_dir_data;
  int error=0;
  uint64_t file_size;
  f_out=fopen_local(&new_file, dir_data->local_dir, dir_data->current_directory);
  if(!f_out)
  {
    log_critical("Can't create file %s: %s\n", new_file, strerror(errno));
    free(new_file);
    return -4;
  }
  log_error("Try to open rfs file %s\n", dir_data->current_directory);
  log_flush();
  in=reiserfs_file_open(ls->current_fs, dir_data->current_directory, O_RDONLY);
  if (in==NULL)
  {
    log_error("Error while opening rfs file %s\n", dir_data->current_directory);
    free(new_file);
    fclose(f_out);
    return -1;
  }
  log_error("open rfs file %s done\n", dir_data->current_directory);
  log_flush();
  file_size = reiserfs_file_size(in);
#if 0
  /* TODO: do not use so much memory */
  {
    void *buf=MALLOC(file_size+1);
    if (reiserfs_file_read(in, buf, file_size) != file_size)
    {
      log_error("Error while reading rfs file %s\n", dir_data->current_directory);
      error = -3;
    }
    else if (fwrite(buf, file_size, 1, f_out) != 1)
    {
      log_error("Error while writing file %s\n", new_file);
      error = -5;
    }
    free(buf);
  }
#else
  {
    /* require progsreiserfs-file-read.patch */
    char buf[4096];
    uint64_t offset=0;
    while(file_size > 0)
    {
      int read_size=(file_size < sizeof(buf) ? file_size : sizeof(buf));
      if (reiserfs_file_read(in, buf, read_size) == 0)
      {
	log_error("Error while reading rfs file %s\n", dir_data->current_directory);
	error = -3;
      }
      else if (fwrite(buf, read_size, 1, f_out) != 1)
      {
	log_error("Error while writing file %s\n", new_file);
	error = -5;
      }
      file_size -= read_size;
      offset += read_size;
    }
  }
#endif
  reiserfs_file_close(in);
  fclose(f_out);
  set_date(new_file, file->td_atime, file->td_mtime);
  set_mode(new_file, file->st_mode);
  free(new_file);
  return error;
}
Esempio n. 7
0
 /**
  * Sets active task to ordered task (or goto if none exists) after
  * goto or aborting.
  */
 void resume() {
   set_mode(MODE_ORDERED);
 }
Esempio n. 8
0
//-----------------push menu-------------------
//sculpting
void option1(){
	set_mode(1);
	Master_ui->remove_menu();
}
Esempio n. 9
0
//selection?
void option3(){
	set_mode(3);
	Master_ui->remove_menu();
}
Esempio n. 10
0
void Wiring_Pin::write_analog(unsigned int value) {
    set_mode(OUTPUT);
    analogWrite(pin, value);
}
Esempio n. 11
0
unsigned int Wiring_Pin::read_analog() {
    set_mode(INPUT);
    return analogRead(pin);
}
Esempio n. 12
0
unsigned int Wiring_Pin::read_digital() {
    set_mode(INPUT);
    return digitalRead(pin);
}
Esempio n. 13
0
void Wiring_Pin::write_digital(unsigned int value) {
    set_mode(OUTPUT);
    digitalWrite(pin, value);
}
Esempio n. 14
0
int
sc_vid_ioctl(struct tty *tp, u_long cmd, caddr_t data, struct thread *td)
{
    scr_stat *scp;
    video_adapter_t *adp;
    video_info_t info;
    video_adapter_info_t adp_info;
    int error;
    int s;
#if defined(COMPAT_FREEBSD6) || defined(COMPAT_FREEBSD5) || \
    defined(COMPAT_FREEBSD4) || defined(COMPAT_43)
    int ival;
#endif

    scp = SC_STAT(tp);
    if (scp == NULL)		/* tp == SC_MOUSE */
	return ENOIOCTL;
    adp = scp->sc->adp;
    if (adp == NULL)		/* shouldn't happen??? */
	return ENODEV;

    switch (cmd) {

    case CONS_CURRENTADP:	/* get current adapter index */
    case FBIO_ADAPTER:
	return fb_ioctl(adp, FBIO_ADAPTER, data);

    case CONS_CURRENT:  	/* get current adapter type */
    case FBIO_ADPTYPE:
	return fb_ioctl(adp, FBIO_ADPTYPE, data);

    case OLD_CONS_ADPINFO:	/* adapter information (old interface) */
	if (((old_video_adapter_t *)data)->va_index >= 0) {
	    adp = vid_get_adapter(((old_video_adapter_t *)data)->va_index);
	    if (adp == NULL)
		return ENODEV;
	}
	((old_video_adapter_t *)data)->va_index = adp->va_index;
	((old_video_adapter_t *)data)->va_type = adp->va_type;
	((old_video_adapter_t *)data)->va_flags = adp->va_flags;
	((old_video_adapter_t *)data)->va_crtc_addr = adp->va_crtc_addr;
	((old_video_adapter_t *)data)->va_window = adp->va_window;
	((old_video_adapter_t *)data)->va_window_size = adp->va_window_size;
	((old_video_adapter_t *)data)->va_window_gran = adp->va_window_gran;
	((old_video_adapter_t *)data)->va_buffer = adp->va_buffer;
	((old_video_adapter_t *)data)->va_buffer_size = adp->va_buffer_size;
	((old_video_adapter_t *)data)->va_mode = adp->va_mode;
	((old_video_adapter_t *)data)->va_initial_mode = adp->va_initial_mode;
	((old_video_adapter_t *)data)->va_initial_bios_mode
	    = adp->va_initial_bios_mode;
	return 0;

    case OLD_CONS_ADPINFO2:	/* adapter information (yet another old I/F) */
	adp_info.va_index = ((old_video_adapter_info_t *)data)->va_index;
	if (adp_info.va_index >= 0) {
	    adp = vid_get_adapter(adp_info.va_index);
	    if (adp == NULL)
		return ENODEV;
	}
	error = fb_ioctl(adp, FBIO_ADPINFO, &adp_info);
	if (error == 0)
	    bcopy(&adp_info, data, sizeof(old_video_adapter_info_t));
	return error;

    case CONS_ADPINFO:		/* adapter information */
    case FBIO_ADPINFO:
	if (((video_adapter_info_t *)data)->va_index >= 0) {
	    adp = vid_get_adapter(((video_adapter_info_t *)data)->va_index);
	    if (adp == NULL)
		return ENODEV;
	}
	return fb_ioctl(adp, FBIO_ADPINFO, data);

    case CONS_GET:      	/* get current video mode */
    case FBIO_GETMODE:
	*(int *)data = scp->mode;
	return 0;

#ifndef SC_NO_MODE_CHANGE
    case FBIO_SETMODE:		/* set video mode */
	if (!(adp->va_flags & V_ADP_MODECHANGE))
 	    return ENODEV;
	info.vi_mode = *(int *)data;
	error = fb_ioctl(adp, FBIO_MODEINFO, &info);
	if (error)
	    return error;
	if (info.vi_flags & V_INFO_GRAPHICS)
	    return sc_set_graphics_mode(scp, tp, *(int *)data);
	else
	    return sc_set_text_mode(scp, tp, *(int *)data, 0, 0, 0, 0);
#endif /* SC_NO_MODE_CHANGE */

    case OLD_CONS_MODEINFO:	/* get mode information (old infterface) */
	info.vi_mode = ((old_video_info_t *)data)->vi_mode;
	error = fb_ioctl(adp, FBIO_MODEINFO, &info);
	if (error == 0)
	    bcopy(&info, (old_video_info_t *)data, sizeof(old_video_info_t));
	return error;

    case CONS_MODEINFO:		/* get mode information */
    case FBIO_MODEINFO:
	return fb_ioctl(adp, FBIO_MODEINFO, data);

    case OLD_CONS_FINDMODE:	/* find a matching video mode (old interface) */
	bzero(&info, sizeof(info));
	bcopy((old_video_info_t *)data, &info, sizeof(old_video_info_t));
	error = fb_ioctl(adp, FBIO_FINDMODE, &info);
	if (error == 0)
	    bcopy(&info, (old_video_info_t *)data, sizeof(old_video_info_t));
	return error;

    case CONS_FINDMODE:		/* find a matching video mode */
    case FBIO_FINDMODE:
	return fb_ioctl(adp, FBIO_FINDMODE, data);

#if defined(COMPAT_FREEBSD6) || defined(COMPAT_FREEBSD5) || \
    defined(COMPAT_FREEBSD4) || defined(COMPAT_43)
    case _IO('c', 104):
	ival = IOCPARM_IVAL(data);
	data = (caddr_t)&ival;
	/* FALLTHROUGH */
#endif
    case CONS_SETWINORG:	/* set frame buffer window origin */
    case FBIO_SETWINORG:
	if (scp != scp->sc->cur_scp)
	    return ENODEV;	/* XXX */
	return fb_ioctl(adp, FBIO_SETWINORG, data);

    case FBIO_GETWINORG:	/* get frame buffer window origin */
	if (scp != scp->sc->cur_scp)
	    return ENODEV;	/* XXX */
	return fb_ioctl(adp, FBIO_GETWINORG, data);

    case FBIO_GETDISPSTART:
    case FBIO_SETDISPSTART:
    case FBIO_GETLINEWIDTH:
    case FBIO_SETLINEWIDTH:
	if (scp != scp->sc->cur_scp)
	    return ENODEV;	/* XXX */
	return fb_ioctl(adp, cmd, data);

    case FBIO_GETPALETTE:
    case FBIO_SETPALETTE:
    case FBIOPUTCMAP:
    case FBIOGETCMAP:
    case FBIOGTYPE:
    case FBIOGATTR:
    case FBIOSVIDEO:
    case FBIOGVIDEO:
    case FBIOSCURSOR:
    case FBIOGCURSOR:
    case FBIOSCURPOS:
    case FBIOGCURPOS:
    case FBIOGCURMAX:
	if (scp != scp->sc->cur_scp)
	    return ENODEV;	/* XXX */
	return fb_ioctl(adp, cmd, data);

    case FBIO_BLANK:
	if (scp != scp->sc->cur_scp)
	    return ENODEV;	/* XXX */
	return fb_ioctl(adp, cmd, data);

#ifndef SC_NO_MODE_CHANGE
    /* generic text modes */
    case SW_TEXT_80x25:	case SW_TEXT_80x30:
    case SW_TEXT_80x43: case SW_TEXT_80x50:
    case SW_TEXT_80x60:
	/* FALLTHROUGH */

    /* VGA TEXT MODES */
    case SW_VGA_C40x25:
    case SW_VGA_C80x25: case SW_VGA_M80x25:
    case SW_VGA_C80x30: case SW_VGA_M80x30:
    case SW_VGA_C80x50: case SW_VGA_M80x50:
    case SW_VGA_C80x60: case SW_VGA_M80x60:
    case SW_VGA_C90x25: case SW_VGA_M90x25:
    case SW_VGA_C90x30: case SW_VGA_M90x30:
    case SW_VGA_C90x43: case SW_VGA_M90x43:
    case SW_VGA_C90x50: case SW_VGA_M90x50:
    case SW_VGA_C90x60: case SW_VGA_M90x60:
    case SW_B40x25:     case SW_C40x25:
    case SW_B80x25:     case SW_C80x25:
    case SW_ENH_B40x25: case SW_ENH_C40x25:
    case SW_ENH_B80x25: case SW_ENH_C80x25:
    case SW_ENH_B80x43: case SW_ENH_C80x43:
    case SW_EGAMONO80x25:

#ifdef PC98
    /* PC98 TEXT MODES */
    case SW_PC98_80x25:
    case SW_PC98_80x30:
#endif
	if (!(adp->va_flags & V_ADP_MODECHANGE))
 	    return ENODEV;
	return sc_set_text_mode(scp, tp, cmd & 0xff, 0, 0, 0, 0);

    /* GRAPHICS MODES */
    case SW_BG320:     case SW_BG640:
    case SW_CG320:     case SW_CG320_D:   case SW_CG640_E:
    case SW_CG640x350: case SW_ENH_CG640:
    case SW_BG640x480: case SW_CG640x480: case SW_VGA_CG320:
    case SW_VGA_MODEX:
#ifdef PC98
    /* PC98 GRAPHICS MODES */
    case SW_PC98_EGC640x400:	case SW_PC98_PEGC640x400:
    case SW_PC98_PEGC640x480:
#endif
	if (!(adp->va_flags & V_ADP_MODECHANGE))
	    return ENODEV;
	return sc_set_graphics_mode(scp, tp, cmd & 0xff);
#endif /* SC_NO_MODE_CHANGE */

#if defined(COMPAT_FREEBSD6) || defined(COMPAT_FREEBSD5) || \
    defined(COMPAT_FREEBSD4) || defined(COMPAT_43)
    case _IO('K', 10):
	ival = IOCPARM_IVAL(data);
	data = (caddr_t)&ival;
	/* FALLTHROUGH */
#endif
    case KDSETMODE:     	/* set current mode of this (virtual) console */
	switch (*(int *)data) {
	case KD_TEXT:   	/* switch to TEXT (known) mode */
	    /*
	     * If scp->mode is of graphics modes, we don't know which
	     * text mode to switch back to...
	     */
	    if (scp->status & GRAPHICS_MODE)
		return EINVAL;
	    /* restore fonts & palette ! */
#if 0
#ifndef SC_NO_FONT_LOADING
	    if (ISFONTAVAIL(adp->va_flags) 
		&& !(scp->status & (GRAPHICS_MODE | PIXEL_MODE)))
		/*
		 * FONT KLUDGE
		 * Don't load fonts for now... XXX
		 */
		if (scp->sc->fonts_loaded & FONT_8)
		    sc_load_font(scp, 0, 8, 8, scp->sc->font_8, 0, 256);
		if (scp->sc->fonts_loaded & FONT_14)
		    sc_load_font(scp, 0, 14, 8, scp->sc->font_14, 0, 256);
		if (scp->sc->fonts_loaded & FONT_16)
		    sc_load_font(scp, 0, 16, 8, scp->sc->font_16, 0, 256);
	    }
#endif /* SC_NO_FONT_LOADING */
#endif

#ifndef SC_NO_PALETTE_LOADING
#ifdef SC_PIXEL_MODE
	    if (adp->va_info.vi_mem_model == V_INFO_MM_DIRECT)
		vidd_load_palette(adp, scp->sc->palette2);
	    else
#endif
	    vidd_load_palette(adp, scp->sc->palette);
#endif

#ifndef PC98
	    /* move hardware cursor out of the way */
	    vidd_set_hw_cursor(adp, -1, -1);
#endif

	    /* FALLTHROUGH */

	case KD_TEXT1:  	/* switch to TEXT (known) mode */
	    /*
	     * If scp->mode is of graphics modes, we don't know which
	     * text/pixel mode to switch back to...
	     */
	    if (scp->status & GRAPHICS_MODE)
		return EINVAL;
	    s = spltty();
	    if ((error = sc_clean_up(scp))) {
		splx(s);
		return error;
	    }
#ifndef PC98
	    scp->status |= UNKNOWN_MODE | MOUSE_HIDDEN;
	    splx(s);
	    /* no restore fonts & palette */
	    if (scp == scp->sc->cur_scp)
		set_mode(scp);
	    sc_clear_screen(scp);
	    scp->status &= ~UNKNOWN_MODE;
#else /* PC98 */
	    scp->status &= ~UNKNOWN_MODE;
	    /* no restore fonts & palette */
	    if (scp == scp->sc->cur_scp)
		set_mode(scp);
	    sc_clear_screen(scp);
	    splx(s);
#endif /* PC98 */
	    return 0;

#ifdef SC_PIXEL_MODE
	case KD_PIXEL:		/* pixel (raster) display */
	    if (!(scp->status & (GRAPHICS_MODE | PIXEL_MODE)))
		return EINVAL;
	    if (scp->status & GRAPHICS_MODE)
		return sc_set_pixel_mode(scp, tp, scp->xsize, scp->ysize, 
					 scp->font_size, scp->font_width);
	    s = spltty();
	    if ((error = sc_clean_up(scp))) {
		splx(s);
		return error;
	    }
	    scp->status |= (UNKNOWN_MODE | PIXEL_MODE | MOUSE_HIDDEN);
	    splx(s);
	    if (scp == scp->sc->cur_scp) {
		set_mode(scp);
#ifndef SC_NO_PALETTE_LOADING
		if (adp->va_info.vi_mem_model == V_INFO_MM_DIRECT)
		    vidd_load_palette(adp, scp->sc->palette2);
		else
		    vidd_load_palette(adp, scp->sc->palette);
#endif
	    }
	    sc_clear_screen(scp);
	    scp->status &= ~UNKNOWN_MODE;
	    return 0;
#endif /* SC_PIXEL_MODE */

	case KD_GRAPHICS:	/* switch to GRAPHICS (unknown) mode */
	    s = spltty();
	    if ((error = sc_clean_up(scp))) {
		splx(s);
		return error;
	    }
	    scp->status |= UNKNOWN_MODE | MOUSE_HIDDEN;
	    splx(s);
#ifdef PC98
	    if (scp == scp->sc->cur_scp)
		set_mode(scp);
#endif
	    return 0;

	default:
	    return EINVAL;
	}
	/* NOT REACHED */

#ifdef SC_PIXEL_MODE
    case KDRASTER:		/* set pixel (raster) display mode */
	if (ISUNKNOWNSC(scp) || ISTEXTSC(scp))
	    return ENODEV;
	return sc_set_pixel_mode(scp, tp, ((int *)data)[0], ((int *)data)[1], 
				 ((int *)data)[2], 8);
#endif /* SC_PIXEL_MODE */

    case KDGETMODE:     	/* get current mode of this (virtual) console */
	/* 
	 * From the user program's point of view, KD_PIXEL is the same 
	 * as KD_TEXT... 
	 */
	*data = ISGRAPHSC(scp) ? KD_GRAPHICS : KD_TEXT;
	return 0;

#if defined(COMPAT_FREEBSD6) || defined(COMPAT_FREEBSD5) || \
    defined(COMPAT_FREEBSD4) || defined(COMPAT_43)
    case _IO('K', 13):
	ival = IOCPARM_IVAL(data);
	data = (caddr_t)&ival;
	/* FALLTHROUGH */
#endif
    case KDSBORDER:     	/* set border color of this (virtual) console */
	scp->border = *(int *)data;
	if (scp == scp->sc->cur_scp)
	    sc_set_border(scp, scp->border);
	return 0;
    }
Esempio n. 15
0
static void print_char(mps_t *mps, unsigned int prnr, const BYTE c)
{
    if (mps->pos >= MAX_COL) {  /* flush buffer*/
        write_line(mps, prnr);
        clear_buffer(mps);
    }
    if (mps->tab) {     /* decode tab-number*/
        mps->tabc[2 - mps->tab] = c;

        if (mps->tab == 1) {
            mps->pos =
                is_mode(mps, MPS_ESC) ?
                mps->tabc[0] << 8 | mps->tabc[1] :
                atoi((char *)mps->tabc) * 6;

            del_mode(mps, MPS_ESC);
        }

        mps->tab--;
        return;
    }

    if (is_mode(mps, MPS_ESC) && (c != 16)) {
        del_mode(mps, MPS_ESC);
    }

    if (is_mode(mps, MPS_REPEAT)) {
        mps->repeatn = c;
        del_mode(mps, MPS_REPEAT);
        return;
    }

    if (is_mode(mps, MPS_BITMODE) && (c & 128)) {
        print_bitmask(mps, c);
        return;
    }

    /* it seems that CR works even in quote mode */
    switch (c) {
        case 13: /* CR*/
            mps->pos = 0;
            if (is_mode(mps, MPS_BUSINESS)) {
                del_mode(mps, MPS_CRSRUP);
            } else {
                set_mode(mps, MPS_CRSRUP);
            }
            /* CR resets Quote mode, revers mode, ... */
            del_mode(mps, MPS_QUOTED);
            del_mode(mps, MPS_REVERSE);
            write_line(mps, prnr);
            clear_buffer(mps);
            return;
    }

    /* in text mode ignore most (?) other control chars when quote mode is active */
    if (!is_mode(mps, MPS_QUOTED) || is_mode(mps, MPS_BITMODE)) {

        switch (c) {
            case 8:
                set_mode(mps, MPS_BITMODE);
                mps->bitcnt = 0;
                return;

            case 10: /* LF*/
                write_line(mps, prnr);
                clear_buffer(mps);
                return;

#ifdef notyet
            /* Not really sure if the MPS803 recognizes this one... */
            case 13 + 128: /* shift CR: CR without LF (from 4023 printer) */
                mps->pos = 0;
                if (is_mode(mps, MPS_BUSINESS)) {
                    del_mode(mps, MPS_CRSRUP);
                } else {
                    set_mode(mps, MPS_CRSRUP);
                }
                /* CR resets Quote mode, revers mode, ... */
                del_mode(mps, MPS_QUOTED);
                del_mode(mps, MPS_REVERSE);
                return;
#endif

            case 14: /* EN on*/
                set_mode(mps, MPS_DBLWDTH);
                if (is_mode(mps, MPS_BITMODE)) {
                    bitmode_off(mps);
                }
                return;

            case 15: /* EN off*/
                del_mode(mps, MPS_DBLWDTH);
                if (is_mode(mps, MPS_BITMODE)) {
                    bitmode_off(mps);
                }
                return;

            case 16: /* POS*/
                mps->tab = 2; /* 2 chars (digits) following, number of first char*/
                return;

            /*
            * By sending the cursor up code [CHR$(145)] to your printer, following
            * characters will be printed in cursor up (graphic) mode until either
            * a carriage return or cursor down code [CHR$(17)] is detected.
            *
            * By sending the cursor down code [CHR$(17)] to your printer,
            * following characters will be printed in business mode until either
            * a carriage return or cursor up code [CHR$(145)] is detected.
            */
            case 17: /* crsr dn, enter businessmode local */
                del_mode(mps, MPS_CRSRUP);
                return;

            case 145: /* CRSR up, enter gfxmode local */
                set_mode(mps, MPS_CRSRUP);
                return;

            case 18:
                set_mode(mps, MPS_REVERSE);
                return;

            case 146: /* 18+128*/
                del_mode(mps, MPS_REVERSE);
                return;

            case 26: /* repeat last chr$(8) c times.*/
                set_mode(mps, MPS_REPEAT);
                mps->repeatn = 0;
                mps->bitcnt = 0;
                return;

            case 27:
                set_mode(mps, MPS_ESC); /* followed by 16, and number MSB, LSB*/
                return;
        }

    }

    if (is_mode(mps, MPS_BITMODE)) {
        return;
    }

   /* 
    * When an odd number of CHR$(34) is detected in a line, the control
    * codes $00-$1F and $80-$9F will be made visible by printing a
    * reverse character for each of these controls. This will continue
    * until an even number of quotes [CHR$(34)] has been received or until
    * end of this line.
    */
    if (c == 34) {
        mps->mode ^= MPS_QUOTED;
    }

    if (is_mode(mps, MPS_QUOTED)) {
        if (c <= 0x1f) {
            set_mode(mps, MPS_REVERSE);
            print_cbm_char(mps, (BYTE)(c + 0x40));
            del_mode(mps, MPS_REVERSE);
            return;
        }
        if ((c >= 0x80) && (c <= 0x9f)) {
            set_mode(mps, MPS_REVERSE);
            print_cbm_char(mps, (BYTE)(c - 0x20));
            del_mode(mps, MPS_REVERSE);
            return;
        }
    }

    print_cbm_char(mps, c);
}
Esempio n. 16
0
// rtl_descent_run - implements the final descent to the RTL_ALT
//      called by rtl_run at 100hz or more
void Copter::rtl_descent_run()
{
    int16_t roll_control = 0, pitch_control = 0;
    float target_yaw_rate = 0;

    // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
    if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw
        // call attitude controller
        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
        attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
        // multicopters do not stabilize roll/pitch/yaw when disarmed
        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
        // set target to current position
        wp_nav.init_loiter_target();
        return;
    }

    // process pilot's input
    if (!failsafe.radio) {
        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
            Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
            // exit land if throttle is high
            if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
                set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
            }
        }

        if (g.land_repositioning) {
            // apply SIMPLE mode transform to pilot inputs
            update_simple_mode();

            // process pilot's roll and pitch input
            roll_control = channel_roll->get_control_in();
            pitch_control = channel_pitch->get_control_in();
        }

        // get pilot's desired yaw rate
        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
    }

    // set motors to full range
    motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

    // process roll, pitch inputs
    wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);

    // run loiter controller
    wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);

    // call z-axis position controller
    pos_control.set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt);
    pos_control.update_z_controller();

    // roll & pitch from waypoint controller, yaw rate from pilot
    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());

    // check if we've reached within 20cm of final altitude
    rtl_state_complete = fabsf(rtl_path.descent_target.alt - current_loc.alt) < 20.0f;
}
Esempio n. 17
0
//--------- Begin of function Unit::change_nation ---------//
//
// This function is called when a unit change nation.
// It is not necessarily a result of betray, when a spy
// hands over his new nation to his parent nation, this
// function will also be called.
//
// <int> newNationRecno - change the nation of the unit.
//
void Unit::change_nation(int newNationRecno)
{
	err_when( newNationRecno && nation_array.is_deleted(newNationRecno) );
	err_when( unit_mode == UNIT_MODE_REBEL );		// rebels do not change nation

	//--- special case: units in Monster Fortress cannot change nation ---//

	if( unit_mode == UNIT_MODE_OVERSEE &&
		 firm_array[unit_mode_para]->firm_id == FIRM_FORTRESS )
	{
		return;
	}

	//---------------------------------//

	int oldAiUnit = is_ai;
	int oldNationRecno = nation_recno;

	//-- if the player is giving a command to this unit, cancel the command --//

   if( nation_recno == nation_array.player_recno &&
		 sprite_recno == unit_array.selected_recno &&
		 power.command_id )
   {
		power.command_id = 0;
   }

	//---------- stop all action to attack this unit ------------//

	unit_array.stop_attack_obj(base_obj_recno);

	//---- update nation_unit_count_array[] ----//

	unit_res[unit_id]->unit_change_nation(newNationRecno, nation_recno, rank_id);

	//------- if the nation has an AI action -------//

	stop_order();       // clear the existing order

	//---------------- update vars ----------------//

//	unit_group_id = unit_array.cur_group_id++;      // separate from the current group
	nation_recno  = newNationRecno;

	home_camp_firm_recno  = 0;					// reset it

	if( race_id )
	{
		nation_contribution = 0;            // contribution to the nation
		total_reward        = 0;
	}
	
	// #### begin Gilbert 24/12 #######//

	// -------- reset royal -------//

	is_royal = 0;

	// #### end Gilbert 24/12 #######//

	//-------- if change to one of the existing nations ------//

	is_ai = nation_recno && nation_array[nation_recno]->is_ai();

	//------------ update AI info --------------//

	if( oldNationRecno )
	{
		Nation* nationPtr = nation_array[oldNationRecno];

		if( rank_id == RANK_GENERAL || rank_id == RANK_KING )
			nationPtr->del_general_info(sprite_recno);

		else if( unit_res[unit_id]->unit_class == UNIT_CLASS_CARAVAN )
			nationPtr->del_caravan_info(sprite_recno);

		else if( unit_res[unit_id]->unit_class == UNIT_CLASS_SHIP )
			nationPtr->del_ship_info(sprite_recno);
	}

	if( nation_recno )
	{
		Nation* nationPtr = nation_array[nation_recno];

		if( rank_id == RANK_GENERAL || rank_id == RANK_KING )
			nationPtr->add_general_info(sprite_recno);

		else if( unit_res[unit_id]->unit_class == UNIT_CLASS_CARAVAN )
			nationPtr->add_caravan_info(sprite_recno);

		else if( unit_res[unit_id]->unit_class == UNIT_CLASS_SHIP )
			nationPtr->add_ship_info(sprite_recno);
	}

	//------ if this unit oversees a firm -----//

	if( unit_mode==UNIT_MODE_OVERSEE )
		firm_array[unit_mode_para]->change_nation(newNationRecno);

	//----- this unit was defending the town before it gets killed ----//

	else if( unit_mode==UNIT_MODE_TOWN_DEFENDER )
	{
		if( !town_array.is_deleted(unit_mode_para) )
			town_array[unit_mode_para]->reduce_defender_count();

		set_mode(0);   // reset unit mode
	}

	//---- if the unit is no longer the same nation as the leader ----//

	if( leader_unit_recno )
	{
		Unit* leaderUnit = unit_array[leader_unit_recno];

		if( leaderUnit->nation_recno != nation_recno )
		{
			err_when( !leaderUnit->team_info );
			leaderUnit->team_info->del_member(sprite_recno);
			leader_unit_recno = 0;
//			team_id = 0;
		}
	}

	// ##### begin Gilbert 9/2 #######//
	explore_area();
	// ##### end Gilbert 9/2 #######//

	//------ if it is currently selected -------//

	if( selected_flag )
		info.disp();

	err_when( spy_recno && spy_array[spy_recno]->cloaked_nation_recno != nation_recno );

	if( spy_recno && spy_array[spy_recno]->cloaked_nation_recno != nation_recno )		// BUGHERE - fix bug on-fly
		spy_array[spy_recno]->cloaked_nation_recno = nation_recno;
}
Esempio n. 18
0
void apollo_kbd_device::kgetchar(UINT8 data)
{
	static const UINT8 ff1116_data[] = { 0x00, 0xff, 0x00 };

	LOG1(("getchar <- %02x", data));

	if (data == 0xff)
	{
		m_rx_message = data;
		putdata(&data, 1);
		m_loopback_mode = 1;
	}
	else if (data == 0x00)
	{
		if (m_loopback_mode)
		{
			set_mode(KBD_MODE_0_COMPATIBILITY);
			m_loopback_mode = 0;
		}
	}
	else
	{
		m_rx_message = m_rx_message << 8 | data;

		switch (m_rx_message)
		{
		case 0xff00:
			putdata(&data, 1);
			m_mode = KBD_MODE_0_COMPATIBILITY;
			m_loopback_mode = 0;
			m_rx_message = 0;
		case 0xff01:
			putdata(&data, 1);
			m_mode = KBD_MODE_1_KEYSTATE;
			m_rx_message = 0;
			break;
		case 0xff11:
			putdata(&data, 1);
			break;
		case 0xff1116:
			putdata(ff1116_data, sizeof(ff1116_data));
			m_loopback_mode = 0;
			m_rx_message = 0;
			break;
		case 0xff1117:
			m_rx_message = 0;
			break;
		case 0xff12:
			putdata(&data, 1);
			break;
		case 0xff1221: // receive ID message
			m_loopback_mode = 0;
			putdata(&data, 1);
			if (keyboard_is_german())
			{
				putstring("3-A\r2-0\rSD-03863-MS\r");
			}
			else
			{
				putstring("3-@\r2-0\rSD-03863-MS\r");
			}

			if (m_mode == KBD_MODE_0_COMPATIBILITY)
			{
				set_mode(KBD_MODE_0_COMPATIBILITY);
			}
			else
			{
				set_mode(KBD_MODE_1_KEYSTATE);
			}
			m_rx_message = 0;
			break;
		case 0xff2181: // beeper on (for 300 ms)
			putdata(&data, 1);
			m_rx_message = 0;
			m_beeper.on();
			break;
		case 0xff2182: // beeper off
			putdata(&data, 1);
			m_rx_message = 0;
			m_beeper.off();
			break;
		default:
			if (m_loopback_mode && data != 0)
			{
				putdata(&data, 1);
			}
			break;
		}
	}
}
Esempio n. 19
0
 /** Set active task to abort mode. */
 void abort() {
   set_mode(MODE_ABORT);
 }
Esempio n. 20
0
int apollo_kbd_device::push_scancode(UINT8 code, UINT8 repeat)
{
	int n_chars = 0;
	UINT16 key_code = 0;
	UINT8 caps = BIT(machine().root_device().ioport("keyboard4")->read(),0);
	UINT8 shift = BIT(machine().root_device().ioport("keyboard4")->read(),1) | BIT(machine().root_device().ioport("keyboard4")->read(),5);
	UINT8 ctrl = BIT(machine().root_device().ioport("keyboard4")->read(),2);
	UINT8 numlock = BIT(machine().root_device().ioport("keyboard4")->read(),6);
	UINT16 index;

	if (keyboard_is_german())
	{
		// map special keys for German keyboard
		switch (code)
		{
		case 0x00: code = 0x68; break; // _
		case 0x0e: code = 0x6b; break; // #
		case 0x29: code = 0x69; break; // <>
		case 0x42: code = 0x6f; break; // NP-
		case 0x46: code = 0x6e; break; // NP+
		case 0x4e: code = 0x73; break; // NP ENTER
		}
	}

#if MAP_APOLLO_KEYS
	if (numlock)
	{
		// don't map function keys to Apollo left keypad
		switch (code)
		{
		case 0x52: code = 0x75; break; // F1
		case 0x53: code = 0x76; break; // F2
		case 0x54: code = 0x77; break; // F3
		case 0x55: code = 0x78; break; // F4
		case 0x56: code = 0x79; break; // F5
		case 0x57: code = 0x7a; break; // F6
		case 0x58: code = 0x7b; break; // F7
		case 0x59: code = 0x7c; break; // F8
		case 0x5a: code = 0x7d; break; // F9
		case 0x5b: code = 0x74; break; // F0 = F10
		}
	}
#endif

	index = (code & 0x7f) * CODE_TABLE_ENTRY_SIZE;
	if (m_mode == KBD_MODE_0_COMPATIBILITY)
	{
		if (code & 0x80)
		{
			// skip up code in ASCII mode
		}
		else if (repeat > 0
				&& m_code_table[index + CODE_TABLE_AUTO_REPEAT_CODE] != Yes)
		{
			// skip repeat in ASCII mode
		}
		else if (ctrl)
		{
			key_code = m_code_table[index + CODE_TABLE_CONTROL_CODE];
		}
		else if (shift)
		{
			key_code = m_code_table[index + CODE_TABLE_SHIFTED_CODE];
		}
		else if (caps)
		{
			key_code = m_code_table[index + CODE_TABLE_CAPS_LOCK_CODE];
		}
		else
		{
			key_code = m_code_table[index + CODE_TABLE_UNSHIFTED_CODE];
		}
	}
	else
	{
		if (repeat > 0)
		{
			if (repeat == 1)
			{
				// auto repeat (but only for first scanned key)
				key_code = 0x7f;
			}
		}
		else if (code & 0x80)
		{
			key_code = m_code_table[index + CODE_TABLE_UP_CODE];
		}
		else
		{
			key_code = m_code_table[index + CODE_TABLE_DOWN_CODE];
		}
	}

	if (key_code != 0)
	{
		LOG2(("scan_code = 0x%02x key_code = 0x%04x",code, key_code));
		if (m_mode > KBD_MODE_1_KEYSTATE)
		{
			set_mode(KBD_MODE_1_KEYSTATE);
		}

		if (key_code & 0xff00)
		{
			xmit_char(key_code >> 8);
			n_chars++;
		}
Esempio n. 21
0
bool IcoPackImageButton::on_leave_notify_event(GdkEventCrossing* event)
{
	bool res = false ;
	set_mode(1) ;
	return res ;
}
Esempio n. 22
0
void Tracker::init_tracker()
{
    // initialise console serial port
    serial_manager.init_console();

    hal.console->printf_P(PSTR("\n\nInit " THISFIRMWARE
                               "\n\nFree RAM: %u\n"),
                          hal.util->available_memory());

    // Check the EEPROM format version before loading any parameters from EEPROM
    load_parameters();

    BoardConfig.init();

    // initialise serial ports
    serial_manager.init();

    // init baro before we start the GCS, so that the CLI baro test works
    barometer.init();

    // init the GCS and start snooping for vehicle data
    gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0);
    gcs[0].set_snoop(mavlink_snoop_static);

    // Register mavlink_delay_cb, which will run anytime you have
    // more than 5ms remaining in your call to hal.scheduler->delay
    hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);

    // we start by assuming USB connected, as we initialed the serial
    // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.    
    usb_connected = true;
    check_usb_mux();

    // setup serial port for telem1 and start snooping for vehicle data
    gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
    gcs[1].set_snoop(mavlink_snoop_static);

#if MAVLINK_COMM_NUM_BUFFERS > 2
    // setup serial port for telem2 and start snooping for vehicle data
    gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1);
    gcs[2].set_snoop(mavlink_snoop_static);
#endif

#if MAVLINK_COMM_NUM_BUFFERS > 3
    // setup serial port for fourth telemetry port (not used by default) and start snooping for vehicle data
    gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2);
    gcs[3].set_snoop(mavlink_snoop_static);
#endif

    mavlink_system.sysid = g.sysid_this_mav;

    if (g.compass_enabled==true) {
        if (!compass.init() || !compass.read()) {
            hal.console->println_P(PSTR("Compass initialisation failed!"));
            g.compass_enabled = false;
        } else {
            ahrs.set_compass(&compass);
        }
    }

    // GPS Initialization
    gps.init(NULL, serial_manager);

    ahrs.init();
    ahrs.set_fly_forward(false);

    ins.init(AP_InertialSensor::WARM_START, ins_sample_rate);
    ahrs.reset();

    init_barometer();

    // set serial ports non-blocking
    serial_manager.set_blocking_writes_all(false);

    // initialise servos
    init_servos();

    // use given start positions - useful for indoor testing, and
    // while waiting for GPS lock
    current_loc.lat = g.start_latitude * 1.0e7f;
    current_loc.lng = g.start_longitude * 1.0e7f;

    // see if EEPROM has a default location as well
    if (current_loc.lat == 0 && current_loc.lng == 0) {
        get_home_eeprom(current_loc);
    }

    gcs_send_text_P(SEVERITY_LOW,PSTR("\nReady to track."));
    hal.scheduler->delay(1000); // Why????

    set_mode(AUTO); // tracking

    if (g.startup_delay > 0) {
        // arm servos with trim value to allow them to start up (required
        // for some servos)
        prepare_servos();
    }

    // calibrate pressure on startup by default
    nav_status.need_altitude_calibration = true;
}
Esempio n. 23
0
int
PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
{
	int ret = OK;

	lock();

	switch (cmd) {
	case PWM_SERVO_ARM:
		up_pwm_servo_arm(true);
		break;

	case PWM_SERVO_SET_ARM_OK:
	case PWM_SERVO_CLEAR_ARM_OK:
	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
		// these are no-ops, as no safety switch
		break;

	case PWM_SERVO_DISARM:
		up_pwm_servo_arm(false);
		break;

	case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
		*(uint32_t *)arg = _pwm_default_rate;
		break;

	case PWM_SERVO_SET_UPDATE_RATE:
		ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
		break;

	case PWM_SERVO_GET_UPDATE_RATE:
		*(uint32_t *)arg = _pwm_alt_rate;
		break;

	case PWM_SERVO_SET_SELECT_UPDATE_RATE:
		ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
		break;

	case PWM_SERVO_GET_SELECT_UPDATE_RATE:
		*(uint32_t *)arg = _pwm_alt_rate_channels;
		break;

	case PWM_SERVO_SET_FAILSAFE_PWM: {
			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;

			/* discard if too many values are sent */
			if (pwm->channel_count > _max_actuators) {
				ret = -EINVAL;
				break;
			}

			for (unsigned i = 0; i < pwm->channel_count; i++) {
				if (pwm->values[i] == 0) {
					/* ignore 0 */
				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
					_failsafe_pwm[i] = PWM_HIGHEST_MAX;

				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
					_failsafe_pwm[i] = PWM_LOWEST_MIN;

				} else {
					_failsafe_pwm[i] = pwm->values[i];
				}
			}

			/*
			 * update the counter
			 * this is needed to decide if disarmed PWM output should be turned on or not
			 */
			_num_failsafe_set = 0;

			for (unsigned i = 0; i < _max_actuators; i++) {
				if (_failsafe_pwm[i] > 0)
					_num_failsafe_set++;
			}

			break;
		}

	case PWM_SERVO_GET_FAILSAFE_PWM: {
			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;

			for (unsigned i = 0; i < _max_actuators; i++) {
				pwm->values[i] = _failsafe_pwm[i];
			}

			pwm->channel_count = _max_actuators;
			break;
		}

	case PWM_SERVO_SET_DISARMED_PWM: {
			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;

			/* discard if too many values are sent */
			if (pwm->channel_count > _max_actuators) {
				ret = -EINVAL;
				break;
			}

			for (unsigned i = 0; i < pwm->channel_count; i++) {
				if (pwm->values[i] == 0) {
					/* ignore 0 */
				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
					_disarmed_pwm[i] = PWM_HIGHEST_MAX;

				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
					_disarmed_pwm[i] = PWM_LOWEST_MIN;

				} else {
					_disarmed_pwm[i] = pwm->values[i];
				}
			}

			/*
			 * update the counter
			 * this is needed to decide if disarmed PWM output should be turned on or not
			 */
			_num_disarmed_set = 0;

			for (unsigned i = 0; i < _max_actuators; i++) {
				if (_disarmed_pwm[i] > 0)
					_num_disarmed_set++;
			}

			break;
		}

	case PWM_SERVO_GET_DISARMED_PWM: {
			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;

			for (unsigned i = 0; i < _max_actuators; i++) {
				pwm->values[i] = _disarmed_pwm[i];
			}

			pwm->channel_count = _max_actuators;
			break;
		}

	case PWM_SERVO_SET_MIN_PWM: {
			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;

			/* discard if too many values are sent */
			if (pwm->channel_count > _max_actuators) {
				ret = -EINVAL;
				break;
			}

			for (unsigned i = 0; i < pwm->channel_count; i++) {
				if (pwm->values[i] == 0) {
					/* ignore 0 */
				} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
					_min_pwm[i] = PWM_HIGHEST_MIN;

				} else if (pwm->values[i] < PWM_LOWEST_MIN) {
					_min_pwm[i] = PWM_LOWEST_MIN;

				} else {
					_min_pwm[i] = pwm->values[i];
				}
			}

			break;
		}

	case PWM_SERVO_GET_MIN_PWM: {
			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;

			for (unsigned i = 0; i < _max_actuators; i++) {
				pwm->values[i] = _min_pwm[i];
			}

			pwm->channel_count = _max_actuators;
			arg = (unsigned long)&pwm;
			break;
		}

	case PWM_SERVO_SET_MAX_PWM: {
			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;

			/* discard if too many values are sent */
			if (pwm->channel_count > _max_actuators) {
				ret = -EINVAL;
				break;
			}

			for (unsigned i = 0; i < pwm->channel_count; i++) {
				if (pwm->values[i] == 0) {
					/* ignore 0 */
				} else if (pwm->values[i] < PWM_LOWEST_MAX) {
					_max_pwm[i] = PWM_LOWEST_MAX;

				} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
					_max_pwm[i] = PWM_HIGHEST_MAX;

				} else {
					_max_pwm[i] = pwm->values[i];
				}
			}

			break;
		}

	case PWM_SERVO_GET_MAX_PWM: {
			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;

			for (unsigned i = 0; i < _max_actuators; i++) {
				pwm->values[i] = _max_pwm[i];
			}

			pwm->channel_count = _max_actuators;
			arg = (unsigned long)&pwm;
			break;
		}

#ifdef CONFIG_ARCH_BOARD_AEROCORE
	case PWM_SERVO_SET(7):
	case PWM_SERVO_SET(6):
		if (_mode < MODE_8PWM) {
			ret = -EINVAL;
			break;
		}
#endif

	case PWM_SERVO_SET(5):
	case PWM_SERVO_SET(4):
		if (_mode < MODE_6PWM) {
			ret = -EINVAL;
			break;
		}

	/* FALLTHROUGH */
	case PWM_SERVO_SET(3):
	case PWM_SERVO_SET(2):
		if (_mode < MODE_4PWM) {
			ret = -EINVAL;
			break;
		}

	/* FALLTHROUGH */
	case PWM_SERVO_SET(1):
	case PWM_SERVO_SET(0):
		if (arg <= 2100) {
			up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);

		} else {
			ret = -EINVAL;
		}

		break;

#ifdef CONFIG_ARCH_BOARD_AEROCORE
	case PWM_SERVO_GET(7):
	case PWM_SERVO_GET(6):
		if (_mode < MODE_8PWM) {
			ret = -EINVAL;
			break;
		}
#endif

	case PWM_SERVO_GET(5):
	case PWM_SERVO_GET(4):
		if (_mode < MODE_6PWM) {
			ret = -EINVAL;
			break;
		}

	/* FALLTHROUGH */
	case PWM_SERVO_GET(3):
	case PWM_SERVO_GET(2):
		if (_mode < MODE_4PWM) {
			ret = -EINVAL;
			break;
		}

	/* FALLTHROUGH */
	case PWM_SERVO_GET(1):
	case PWM_SERVO_GET(0):
		*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
		break;

	case PWM_SERVO_GET_RATEGROUP(0):
	case PWM_SERVO_GET_RATEGROUP(1):
	case PWM_SERVO_GET_RATEGROUP(2):
	case PWM_SERVO_GET_RATEGROUP(3):
	case PWM_SERVO_GET_RATEGROUP(4):
	case PWM_SERVO_GET_RATEGROUP(5):
#ifdef CONFIG_ARCH_BOARD_AEROCORE
	case PWM_SERVO_GET_RATEGROUP(6):
	case PWM_SERVO_GET_RATEGROUP(7):
#endif
		*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
		break;

	case PWM_SERVO_GET_COUNT:
	case MIXERIOCGETOUTPUTCOUNT:
		switch (_mode) {
#ifdef CONFIG_ARCH_BOARD_AEROCORE
		case MODE_8PWM:
			*(unsigned *)arg = 8;
			break;
#endif

		case MODE_6PWM:
			*(unsigned *)arg = 6;
			break;

		case MODE_4PWM:
			*(unsigned *)arg = 4;
			break;

		case MODE_2PWM:
			*(unsigned *)arg = 2;
			break;

		default:
			ret = -EINVAL;
			break;
		}

		break;

	case PWM_SERVO_SET_COUNT: {
		/* change the number of outputs that are enabled for
		 * PWM. This is used to change the split between GPIO
		 * and PWM under control of the flight config
		 * parameters. Note that this does not allow for
		 * changing a set of pins to be used for serial on
		 * FMUv1 
		 */
		switch (arg) {
		case 0:
			set_mode(MODE_NONE);
			break;

		case 2:
			set_mode(MODE_2PWM);
			break;

		case 4:
			set_mode(MODE_4PWM);
			break;

#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
		case 6:
			set_mode(MODE_6PWM);
			break;
#endif
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
		case 8:
			set_mode(MODE_8PWM);
			break;
#endif

		default:
			ret = -EINVAL;
			break;
		}
		break;
	}

	case MIXERIOCRESET:
		if (_mixers != nullptr) {
			delete _mixers;
			_mixers = nullptr;
			_groups_required = 0;
		}

		break;

	case MIXERIOCADDSIMPLE: {
			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;

			SimpleMixer *mixer = new SimpleMixer(control_callback,
							     (uintptr_t)_controls, mixinfo);

			if (mixer->check()) {
				delete mixer;
				_groups_required = 0;
				ret = -EINVAL;

			} else {
				if (_mixers == nullptr)
					_mixers = new MixerGroup(control_callback,
								 (uintptr_t)_controls);

				_mixers->add_mixer(mixer);
				_mixers->groups_required(_groups_required);
			}

			break;
		}

	case MIXERIOCLOADBUF: {
			const char *buf = (const char *)arg;
			unsigned buflen = strnlen(buf, 1024);

			if (_mixers == nullptr)
				_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);

			if (_mixers == nullptr) {
				_groups_required = 0;
				ret = -ENOMEM;

			} else {

				ret = _mixers->load_from_buf(buf, buflen);

				if (ret != 0) {
					debug("mixer load failed with %d", ret);
					delete _mixers;
					_mixers = nullptr;
					_groups_required = 0;
					ret = -EINVAL;
				} else {

					_mixers->groups_required(_groups_required);
				}
			}

			break;
		}

	default:
		ret = -ENOTTY;
		break;
	}

	unlock();

	return ret;
}
Esempio n. 24
0
// set_mode_to_default - restores the mode to it's default mode held in the MNT_MODE parameter
//      this operation requires 230us on an APM2, 60us on a Pixhawk/PX4
void AP_Mount::set_mode_to_default(uint8_t instance)
{
    set_mode(instance, (enum MAV_MOUNT_MODE)state[instance]._default_mode.get());
}
Esempio n. 25
0
main() {
	bool flat = false;
	//int *low, *high;
	vector_t light;

	surface_t screen;

	// this is a vector buffer, for the transformations
	// our only object have 5 vertexes, but we'll make space for 32 anyway
	vector_t *pbuffer;

	// off-screen surface buffer
	//u_char* sbuffer = (u_char*)malloc(MODE2_MAX);

	// our solid :)
	object_t triangle;


	heapinit (HPSIZE);
	
	pbuffer = newa(vector_t, 32);

	triangle.mesh = build_mesh();
	triangle.rot_x = triangle.rot_y = triangle.rot_z = 0;
	triangle.trans_x = triangle.trans_y = 0;
	triangle.trans_z = i2f(30);	// remember: we are using fixed-point numbers

	screen.data.ram = sbuffer;

	// polygon rendering buffers
	//low = newa(int, MODE2_HEIGHT);
	//high = newa(int, MODE2_HEIGHT);

	// light source
	light.x = light.y = light.z = i2f(1);
	vector_normalize(&light, &light);

	printf("spinning solid demo\n\n");

	printf("instructions:\n   press [UP] to toggle flat shading\n\n");

	printf("creating look-up tables, please wait\n");
	create_lookup_tables();

	// set screen to graphic mode
	set_color(15, 1, 1);
	set_mode(mode_2);
	fill(MODE2_ATTR, 0xF1, MODE2_MAX);

	//surface_line(&screen, 0, 0, 0, 0); // FIXME: won't compile without this crap

	while (!get_trigger(0)) {
		if (get_stick(0) == 1)
			flat = !flat;

		// rotate a bit
		triangle.rot_y += 2;
		triangle.rot_x += 3;
		triangle.rot_z += 1;

		// clear the off-screen buffer
		memset(sbuffer, 0, MODE2_MAX);	// [*] 

	//surface_line(screen, 0, 0, 10, 10);

		// render the object
		if (flat)
			//object_render_flatshading(&screen, &triangle, pbuffer, low, high, &light);
			object_render_flatshading(&screen, &triangle, pbuffer, stencil, &light);
		else
			object_render_wireframe(&screen, &triangle, pbuffer);

		// show the off-screen buffer
		//vwrite(screen.data.ram, 0, MODE2_MAX); // [*]
		msx_vwrite_direct(screen.data.ram, 0, MODE2_MAX);

		// [*] FIXME: there will be better ways of doing this (soon)
	}

	// go back to text mode
	set_mode(mode_0);

	// deallocate stuff

	mesh_delete(triangle.mesh);
	//free(sbuffer);
	//free(low);
	//free(high);
	//destroy_lookup_tables();
}
Esempio n. 26
0
void t5()
{
   int BAUD=4800;

   char *TEXT=
"\n\
Now is the winter of our discontent\n\
Made glorious summer by this sun of York;\n\
And all the clouds that lour'd upon our house\n\
In the deep bosom of the ocean buried.\n\
Now are our brows bound with victorious wreaths;\n\
Our bruised arms hung up for monuments;\n\
Our stern alarums changed to merry meetings,\n\
Our dreadful marches to delightful measures.\n\
Grim-visaged war hath smooth'd his wrinkled front;\n\
And now, instead of mounting barded steeds\n\
To fright the souls of fearful adversaries,\n\
He capers nimbly in a lady's chamber\n\
To the lascivious pleasing of a lute.\n\
";

   gpioPulse_t wf[] =
   {
      {1<<GPIO, 0,  10000},
      {0, 1<<GPIO,  30000},
      {1<<GPIO, 0,  60000},
      {0, 1<<GPIO, 100000},
   };

   int e, oc, c;

   char text[2048];

   printf("Waveforms & serial read/write tests.\n");

   callback(GPIO, FALLING_EDGE, t5cbf);

   set_mode(GPIO, PI_OUTPUT);

   e = wave_clear();
   CHECK(5, 1, e, 0, 0, "callback, set mode, wave clear");

   e = wave_add_generic(4, wf);
   CHECK(5, 2, e, 4, 0, "pulse, wave add generic");

   e = wave_tx_repeat();
   CHECK(5, 3, e, 9, 0, "wave tx repeat");

   oc = t5_count;
   time_sleep(5);
   c = t5_count - oc;
   CHECK(5, 4, c, 50, 1, "callback");

   e = wave_tx_stop();
   CHECK(5, 5, e, 0, 0, "wave tx stop");

   e = serial_read_open(GPIO, BAUD);
   CHECK(5, 6, e, 0, 0, "serial read open");

   wave_clear();
   e = wave_add_serial(GPIO, BAUD, 5000000, strlen(TEXT), TEXT);
   CHECK(5, 7, e, 3405, 0, "wave clear, wave add serial");

   e = wave_tx_start();
   CHECK(5, 8, e, 6811, 0, "wave tx start");

   oc = t5_count;
   time_sleep(3);
   c = t5_count - oc;
   CHECK(5, 9, c, 0, 0, "callback");

   oc = t5_count;
   while (wave_tx_busy()) time_sleep(0.1);
   time_sleep(0.1);
   c = t5_count - oc;
   CHECK(5, 10, c, 1702, 0, "wave tx busy, callback");

   c = serial_read(GPIO, text, sizeof(text)-1);
   if (c > 0) text[c] = 0; /* null terminate string */
   CHECK(5, 11, strcmp(TEXT, text), 0, 0, "wave tx busy, serial read");

   e = serial_read_close(GPIO);
   CHECK(5, 12, e, 0, 0, "serial read close");

   c = wave_get_micros();
   CHECK(5, 13, c, 6158704, 0, "wave get micros");

   c = wave_get_high_micros();
   CHECK(5, 14, c, 6158704, 0, "wave get high micros");

   c = wave_get_max_micros();
   CHECK(5, 15, c, 1800000000, 0, "wave get max micros");

   c = wave_get_pulses();
   CHECK(5, 16, c, 3405, 0, "wave get pulses");

   c = wave_get_high_pulses();
   CHECK(5, 17, c, 3405, 0, "wave get high pulses");

   c = wave_get_max_pulses();
   CHECK(5, 18, c, 12000, 0, "wave get max pulses");

   c = wave_get_cbs();
   CHECK(5, 19, c, 6810, 0, "wave get cbs");

   c = wave_get_high_cbs();
   CHECK(5, 20, c, 6810, 0, "wave get high cbs");

   c = wave_get_max_cbs();
   CHECK(5, 21, c, 25016, 0, "wave get max cbs");
}
Esempio n. 27
0
// auto_land_run - lands in auto mode
//      called by auto_run at 100hz or more
void Copter::auto_land_run()
{
    int16_t roll_control = 0, pitch_control = 0;
    float target_yaw_rate = 0;

    // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
    if (!motors.armed() || !ap.auto_armed || ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw
        // call attitude controller
        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
        attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
        motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
        // multicopters do not stabilize roll/pitch/yaw when disarmed
        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
        // set target to current position
        wp_nav.init_loiter_target();
        return;
    }

    // relax loiter targets if we might be landed
    if (ap.land_complete_maybe) {
        wp_nav.loiter_soften_for_landing();
    }

    // process pilot's input
    if (!failsafe.radio) {
        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
            Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
            // exit land if throttle is high
            if (!set_mode(LOITER)) {
                set_mode(ALT_HOLD);
            }
        }

        if (g.land_repositioning) {
            // apply SIMPLE mode transform to pilot inputs
            update_simple_mode();

            // process pilot's roll and pitch input
            roll_control = channel_roll->control_in;
            pitch_control = channel_pitch->control_in;
        }

        // get pilot's desired yaw rate
        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
    }

    // set motors to full range
    motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

    // process roll, pitch inputs
    wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);

    // run loiter controller
    wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);

    // call z-axis position controller
    float cmb_rate = get_land_descent_speed();
    pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
    pos_control.update_z_controller();

    // record desired climb rate for logging
    desired_climb_rate = cmb_rate;

    // roll & pitch from waypoint controller, yaw rate from pilot
    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
}
Esempio n. 28
0
void *main_thread( void )
{
    unsigned int i, j, frame = 0, bgColor = 230, counter = 0, cycleOn = 0;
  
    set_mode( VGA_MODE );
  
    //Initialization
    initialize_keyboardISR( );
    intro( );
    xvideo_rect_fill( 0, 0, SCREEN_WIDTH - 1, SCREEN_HEIGHT - 1, bgColor );
  
    while( 1 ) {
        if( getKey( ) != 0 ) cycleOn = !cycleOn;
        xvideo_rect_fill( 144, 84, 176, 116, bgColor );
        switch( frame ) {
        case 0:
            xvideo_put_trans_image( 144, 84, 0x6B, "run1" );
            break;
        case 1:
            xvideo_put_trans_image( 144, 84, 0x6B, "run2" );
            break;
        case 2:
            xvideo_put_trans_image( 144, 84, 0x6B, "run3" );
            break;
        case 3:
            xvideo_put_trans_image( 144, 84, 0x6B, "run4" );
            break;
        case 4:
            xvideo_put_trans_image( 144, 84, 0x6B, "run5" );
            break;
        case 5:
            xvideo_put_trans_image( 144, 84, 0x6B, "run6" );
            break;
        case 6:
            xvideo_put_trans_image( 144, 84, 0x6B, "run7" );
            break;
        case 7:
            xvideo_put_trans_image( 144, 84, 0x6B, "run8" );
            break;
        }
        frame++;
        if( frame > 7 ) frame = 0;
    
        //Delay
        for( i = 0; i < 4000; i++ ) {
            for( j = 0; j < 5000; j++ ) { }
        }
    
        if( cycleOn ) {
            counter++;
            if( counter == 16 ) {
                counter = 0;
                bgColor++;
                if( bgColor > 255 ) {
                    bgColor = 230;
                }
                xvideo_rect_fill( 0, 0, SCREEN_WIDTH - 1, SCREEN_HEIGHT - 1, bgColor );
            }
        }
    }
    return NULL;
}
Esempio n. 29
0
/*
  update navigation for landing. Called when on landing approach or
  final flare
 */
bool Plane::verify_land()
{
    // we don't 'verify' landing in the sense that it never completes,
    // so we don't verify command completion. Instead we use this to
    // adjust final landing parameters

    // when aborting a landing, mimic the verify_takeoff with steering hold. Once
    // the altitude has been reached, restart the landing sequence
    if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {

        throttle_suppressed = false;
        auto_state.land_complete = false;
        auto_state.land_pre_flare = false;
        nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc));

        // see if we have reached abort altitude
        if (adjusted_relative_altitude_cm() > auto_state.takeoff_altitude_rel_cm) {
            next_WP_loc = current_loc;
            mission.stop();
            bool success = restart_landing_sequence();
            mission.resume();
            if (!success) {
                // on a restart failure lets RTL or else the plane may fly away with nowhere to go!
                set_mode(RTL, MODE_REASON_MISSION_END);
            }
            // make sure to return false so it leaves the mission index alone
        }
        return false;
    }

    float height = height_above_target();

    // use rangefinder to correct if possible
    height -= rangefinder_correction();

    /* Set land_complete (which starts the flare) under 3 conditions:
       1) we are within LAND_FLARE_ALT meters of the landing altitude
       2) we are within LAND_FLARE_SEC of the landing point vertically
          by the calculated sink rate (if LAND_FLARE_SEC != 0)
       3) we have gone past the landing point and don't have
          rangefinder data (to prevent us keeping throttle on 
          after landing if we've had positive baro drift)
    */
#if RANGEFINDER_ENABLED == ENABLED
    bool rangefinder_in_range = rangefinder_state.in_range;
#else
    bool rangefinder_in_range = false;
#endif

    // flare check:
    // 1) below flare alt/sec requires approach stage check because if sec/alt are set too
    //    large, and we're on a hard turn to line up for approach, we'll prematurely flare by
    //    skipping approach phase and the extreme roll limits will make it hard to line up with runway
    // 2) passed land point and don't have an accurate AGL
    // 3) probably crashed (ensures motor gets turned off)

    bool on_approach_stage = (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
                              flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE);
    bool below_flare_alt = (height <= g.land_flare_alt);
    bool below_flare_sec = (aparm.land_flare_sec > 0 && height <= auto_state.sink_rate * aparm.land_flare_sec);
    bool probably_crashed = (g.crash_detection_enable && fabsf(auto_state.sink_rate) < 0.2f && !is_flying());

    if ((on_approach_stage && below_flare_alt) ||
        (on_approach_stage && below_flare_sec && (auto_state.wp_proportion > 0.5)) ||
        (!rangefinder_in_range && auto_state.wp_proportion >= 1) ||
        probably_crashed) {

        if (!auto_state.land_complete) {
            auto_state.post_landing_stats = true;
            if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) {
                gcs_send_text_fmt(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)gps.ground_speed());
            } else {
                gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f",
                                  (double)height, (double)auto_state.sink_rate,
                                  (double)gps.ground_speed(),
                                  (double)get_distance(current_loc, next_WP_loc));
            }
            auto_state.land_complete = true;
            update_flight_stage();
        }


        if (gps.ground_speed() < 3) {
            // reload any airspeed or groundspeed parameters that may have
            // been set for landing. We don't do this till ground
            // speed drops below 3.0 m/s as otherwise we will change
            // target speeds too early.
            g.airspeed_cruise_cm.load();
            g.min_gndspeed_cm.load();
            aparm.throttle_cruise.load();
        }
    } else if (!auto_state.land_complete && !auto_state.land_pre_flare && aparm.land_pre_flare_airspeed > 0) {
        bool reached_pre_flare_alt = g.land_pre_flare_alt > 0 && (height <= g.land_pre_flare_alt);
        bool reached_pre_flare_sec = g.land_pre_flare_sec > 0 && (height <= auto_state.sink_rate * g.land_pre_flare_sec);
        if (reached_pre_flare_alt || reached_pre_flare_sec) {
            auto_state.land_pre_flare = true;
            update_flight_stage();
        }
    }

    /*
      when landing we keep the L1 navigation waypoint 200m ahead. This
      prevents sudden turns if we overshoot the landing point
     */
    struct Location land_WP_loc = next_WP_loc;
	int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
    location_update(land_WP_loc,
                    land_bearing_cd*0.01f, 
                    get_distance(prev_WP_loc, current_loc) + 200);
    nav_controller->update_waypoint(prev_WP_loc, land_WP_loc);

    // once landed and stationary, post some statistics
    // this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm
    if (auto_state.post_landing_stats && !arming.is_armed()) {
        auto_state.post_landing_stats = false;
        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)get_distance(current_loc, next_WP_loc));
    }

    // check if we should auto-disarm after a confirmed landing
    disarm_if_autoland_complete();

    /*
      we return false as a landing mission item never completes

      we stay on this waypoint unless the GCS commands us to change
      mission item, reset the mission, command a go-around or finish
      a land_abort procedure.
     */
    return false;
}
Esempio n. 30
0
int
sc_set_text_mode(scr_stat *scp, struct tty *tp, int mode, int xsize, int ysize,
		 int fontsize, int fontwidth)
{
    video_info_t info;
    u_char *font;
    int prev_ysize;
    int error;
    int s;

    if (vidd_get_info(scp->sc->adp, mode, &info))
	return ENODEV;

    /* adjust argument values */
    if (fontwidth <= 0)
	fontwidth = info.vi_cwidth;
    if (fontsize <= 0)
	fontsize = info.vi_cheight;
    if (fontsize < 14)
	fontsize = 8;
    else if (fontsize >= 16)
	fontsize = 16;
    else
	fontsize = 14;
#ifndef SC_NO_FONT_LOADING
    switch (fontsize) {
    case 8:
	if ((scp->sc->fonts_loaded & FONT_8) == 0)
	    return (EINVAL);
	font = scp->sc->font_8;
	break;
    case 14:
	if ((scp->sc->fonts_loaded & FONT_14) == 0)
	    return (EINVAL);
	font = scp->sc->font_14;
	break;
    case 16:
	if ((scp->sc->fonts_loaded & FONT_16) == 0)
	    return (EINVAL);
	font = scp->sc->font_16;
	break;
    }
#else
    font = NULL;
#endif
    if ((xsize <= 0) || (xsize > info.vi_width))
	xsize = info.vi_width;
    if ((ysize <= 0) || (ysize > info.vi_height))
	ysize = info.vi_height;

    /* stop screen saver, etc */
    s = spltty();
    if ((error = sc_clean_up(scp))) {
	splx(s);
	return error;
    }

    if (sc_render_match(scp, scp->sc->adp->va_name, 0) == NULL) {
	splx(s);
	return ENODEV;
    }

    /* set up scp */
#ifndef SC_NO_HISTORY
    if (scp->history != NULL)
	sc_hist_save(scp);
#endif
    prev_ysize = scp->ysize;
    /*
     * This is a kludge to fend off scrn_update() while we
     * muck around with scp. XXX
     */
    scp->status |= UNKNOWN_MODE | MOUSE_HIDDEN;
    scp->status &= ~(GRAPHICS_MODE | PIXEL_MODE | MOUSE_VISIBLE);
    scp->mode = mode;
    scp->xsize = xsize;
    scp->ysize = ysize;
    scp->xoff = 0;
    scp->yoff = 0;
    scp->xpixel = scp->xsize*8;
    scp->ypixel = scp->ysize*fontsize;
    scp->font = font;
    scp->font_size = fontsize;
    scp->font_width = fontwidth;

    /* allocate buffers */
    sc_alloc_scr_buffer(scp, TRUE, TRUE);
    sc_init_emulator(scp, NULL);
#ifndef SC_NO_CUTPASTE
    sc_alloc_cut_buffer(scp, FALSE);
#endif
#ifndef SC_NO_HISTORY
    sc_alloc_history_buffer(scp, 0, prev_ysize, FALSE);
#endif
    splx(s);

    if (scp == scp->sc->cur_scp)
	set_mode(scp);
    scp->status &= ~UNKNOWN_MODE;

    if (tp == NULL)
	return 0;
    DPRINTF(5, ("ws_*size (%d,%d), size (%d,%d)\n",
	tp->t_winsize.ws_col, tp->t_winsize.ws_row, scp->xsize, scp->ysize));
    if (tp->t_winsize.ws_col != scp->xsize
	|| tp->t_winsize.ws_row != scp->ysize) {
	tp->t_winsize.ws_col = scp->xsize;
	tp->t_winsize.ws_row = scp->ysize;

	tty_signal_pgrp(tp, SIGWINCH);
    }

    return 0;
}