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 */
}

// 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;
//.........这里部分代码省略.........

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;
//.........这里部分代码省略.........

bool IcoPackImageButton::on_button_press_event(GdkEventButton* event)
{
	bool res = false ;
	set_mode(3) ;
	return res ;
}

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);
//.........这里部分代码省略.........

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

 /**
  * Sets active task to ordered task (or goto if none exists) after
  * goto or aborting.
  */
 void resume() {
   set_mode(MODE_ORDERED);
 }

//-----------------push menu-------------------
//sculpting
void option1(){
	set_mode(1);
	Master_ui->remove_menu();
}

//selection?
void option3(){
	set_mode(3);
	Master_ui->remove_menu();
}

void Wiring_Pin::write_analog(unsigned int value) {
    set_mode(OUTPUT);
    analogWrite(pin, value);
}

//.........这里部分代码省略.........
#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

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;
//.........这里部分代码省略.........

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

//.........这里部分代码省略.........

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

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("[email protected]\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;
		}
	}
}

 /** Set active task to abort mode. */
 void abort() {
   set_mode(MODE_ABORT);
 }

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);
		}
//.........这里部分代码省略.........

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;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
			*(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;

// 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());
}