Beispiel #1
0
Datei: Atlas.c Projekt: ddeo/sdp
/**********************************************************************

 * @return None
 * @remark Steps into the set station state machine.
 **********************************************************************/
 static void doSetStationSM() {
    /* Decide whether to save the current postion as a station, or
        a given position. */
    if (wantSaveStation) {
        // Save current position as station
        Navigation_getLocalPosition(&nedStation);

        handleAcknowledgement();
        haveStation = TRUE;
        event.flags.setStationDone = TRUE;

        DBPRINT("Saved new station: N=%.2f, E=%.2f, D=%.2f\n",
            nedStation.north, nedStation.east, nedStation.down);
     }
     else {
         // Set the given coordinate as station
        nedStation.north = Mavlink_newMessage.gpsLocalData.north;
        nedStation.east = Mavlink_newMessage.gpsLocalData.east;
        nedStation.down = Mavlink_newMessage.gpsLocalData.down;

        handleAcknowledgement();
        haveStation = TRUE;
        event.flags.setStationDone = TRUE;
        
        DBPRINT("Set new station: N=%.2f, E=%.2f, D=%.2f.\n",
            nedStation.north, nedStation.east, nedStation.down);
    }
}
Beispiel #2
0
void destroy_thread(thread_t *thread)
{
    if (!thread) {
        return;
    }
    --num_threads;

    DBPRINT("\033\012%s Freeing kstack %x ", thread->ustack ? "User" : "Kernel", thread->kstack);
    kfree((void *)thread->kstack);

    if (thread->ustack != 0) {
        /*uint32_t *stack = (uint32_t *)thread->kstack;
        (void)stack;
        DBPRINT("--- thread->kstack: %x\n", thread->kstack);
        for (int i = -15; i < 15; ++i) {
            DBPRINT("%x -> stack[%d] = %x\n", thread->kstack + i, i, stack[i]);
        } */
    
        DBPRINT("- Freeing ustack %x ", thread->ustack);
        kfree((void *)thread->ustack);
    }

    DBPRINT("- Freeing thread %x\033\017\n", thread);
    kfree(thread);
}
Beispiel #3
0
int main(){
    Board_init();
    Board_configure(USE_TIMER);
    DELAY(10);
    DBPRINT("Starting XBee...\n");
    if (Xbee_init(XBEE_UART_ID) != SUCCESS) {
        DBPRINT("Failed XBee init.\n");
        return FAILURE;
    }
    DBPRINT("XBee initialized.\n");
    DELAY(STARTUP_DELAY);

    Timer_new(TIMER_TEST, PRINT_DELAY);
    unsigned long int sent = 0;
    unsigned long int got = 0;
    while(1){
        Xbee_runSM();
        if (Timer_isExpired(TIMER_TEST)) {
            //dbprint("XBee: Sent=%ld, Got=%ld\n", ++sent, got);
            Mavlink_sendRequestOrigin();
            Timer_new(TIMER_TEST, PRINT_DELAY);
        }
        if (Mavlink_hasNewMessage()) {
            ++got;
        }
    }

    return SUCCESS;
}
Beispiel #4
0
char Accelerometer_init() {
    int16_t c = readRegister(WHO_AM_I_ADDRESS);  // Read WHO_AM_I register
    if (c == WHO_AM_I_VALUE) //c != ERROR ||
    {
        DBPRINT("Accelerometer is online...\n");
    }
    else
    {
        DBPRINT("Accelerometer: Failed to connect (0x%X).\n",(int16_t)c);
        return FAILURE;
    }

    setStandbyMode(); // Must be in standby to change registers

    // Set up the full scale range to 2, 4, or 8g.
    char fsr = GSCALE;
    if(fsr > 8) fsr = 8; // Limit G-Scale
    fsr >>= 2; // 00 = 2G, 01 = 4A, 10 = 8G (pg. 20)
    writeRegister(XYZ_DATA_CFG_ADDRESS, fsr);

    setActiveMode();
    Timer_new(TIMER_ACCELEROMETER, UPDATE_DELAY);

#ifdef USE_ACCUMULATOR
    resetAccumulator();
#endif
    haveReading = FALSE;
    
    return SUCCESS;
}
Beispiel #5
0
/*
 * This function replaces sys/dev/cninit.c
 * Determine which device is the console using
 * the PROM "input source" and "output sink".
 */
void
consinit(void)
{
	char buffer[128];
	const char *consname = "unknown";

	DBPRINT(("consinit()\r\n"));

	if (cn_tab != &consdev_prom)
		return;

	if ((prom_stdin_node = prom_instance_to_package(prom_stdin())) == 0) {
		printf("WARNING: no PROM stdin\n");
	}
	DBPRINT(("stdin node = %x\r\n", prom_stdin_node));

	if ((prom_stdout_node = prom_instance_to_package(prom_stdout())) == 0)
		printf("WARNING: no PROM stdout\n");
	DBPRINT(("stdout package = %x\r\n", prom_stdout_node));

	DBPRINT(("buffer @ %p\r\n", buffer));

	if (prom_stdin_node != 0 &&
	    (prom_getproplen(prom_stdin_node, "keyboard") >= 0)) {
#if NUKBD > 0
		if ((OF_instance_to_path(prom_stdin(), buffer, sizeof(buffer)) >= 0) &&
		    (strstr(buffer, "/usb@") != NULL)) {
			/*
		 	* If we have a USB keyboard, it will show up as (e.g.)
		 	*   /pci@1f,0/usb@c,3/keyboard@1	(Blade 100)
		 	*/
			consname = "usb-keyboard/display";
			ukbd_cnattach();
		} else
#endif
			consname = "sun-keyboard/display";
	} else if (prom_stdin_node != 0 &&
		   (OF_instance_to_path(prom_stdin(), buffer, sizeof(buffer)) >= 0)) {
		consname = buffer;
	}
	DBPRINT(("console is %s\n", consname));
#ifndef DEBUG
	(void)consname;
#endif

	/* Initialize PROM console */
	(*cn_tab->cn_probe)(cn_tab);
	(*cn_tab->cn_init)(cn_tab);
}
Beispiel #6
0
Compass::Compass(WorldState* ms, float minMagCal[], float maxMagCal[])
{
  myState = ms;
  compass = new Adafruit_LSM303();

  calMin.x = minMagCal[0];
  calMin.y = minMagCal[1];
  calMin.z = minMagCal[2];
  calMax.x = maxMagCal[0];
  calMax.y = maxMagCal[1];
  calMax.z = maxMagCal[2];

  DBPRINT("Compass cal min: ");DBPRINT(calMin.x);DBPRINT(",");DBPRINT(calMin.y);DBPRINT(",");DBPRINT(calMin.z);
  DBPRINT("\n            max: ");DBPRINT(calMax.x);DBPRINT(",");DBPRINT(calMax.y);DBPRINT(",");DBPRINTLN(calMax.z);
}
Beispiel #7
0
static void ipmr_cache_resolve(struct mfc_cache *uc, struct mfc_cache *c)
{
	struct sk_buff *skb;

	/*
	 *	Play the pending entries through our router
	 */

	while((skb=__skb_dequeue(&uc->mfc_un.unres.unresolved))) {
		if (skb->nh.iph->version == 0) {
			int err;
			struct nlmsghdr *nlh = (struct nlmsghdr *)skb_pull(skb, sizeof(struct iphdr));

			if (ipmr_fill_mroute(skb, c, NLMSG_DATA(nlh)) > 0) {
				nlh->nlmsg_len = skb->tail - (u8*)nlh;
			} else {
				nlh->nlmsg_type = NLMSG_ERROR;
				nlh->nlmsg_len = NLMSG_LENGTH(sizeof(struct nlmsgerr));
				skb_trim(skb, nlh->nlmsg_len);
				((struct nlmsgerr*)NLMSG_DATA(nlh))->error = -EMSGSIZE;
			}
			err = netlink_unicast(rtnl, skb, NETLINK_CB(skb).dst_pid, MSG_DONTWAIT);
		} else {
	 		DBPRINT("ipmr_cache_resolve - IP packet from %u.%u.%u.%u -> %u.%u.%u.%u with NextProtocol= %d\n",
			NIPQUAD(skb->nh.iph->saddr), NIPQUAD(skb->nh.iph->daddr), skb->nh.iph->protocol);
			ip_mr_forward(skb, c, 0);
		}
	}
}
Beispiel #8
0
Datei: Atlas.c Projekt: ddeo/sdp
/**********************************************************************
 * Function: doSetOriginSM
 * @return None
 * @remark Steps into the set origin state machine.
 **********************************************************************/
static void doSetOriginSM() {
    // Waiting for origin message from command center 
    if (event.flags.haveSetOriginMessage) {
        GeocentricCoordinate ecefOrigin;
        ecefOrigin.x = Mavlink_newMessage.gpsGeocentricData.x;
        ecefOrigin.y = Mavlink_newMessage.gpsGeocentricData.y;
        ecefOrigin.z = Mavlink_newMessage.gpsGeocentricData.z;
        Navigation_setOrigin(&ecefOrigin);

        handleAcknowledgement();
        haveOrigin = TRUE;
        event.flags.setOriginDone = TRUE;

        DBPRINT("Set new origin: X=%.1f, Y=%.1f, Z=%.1f\n",
            ecefOrigin.x, ecefOrigin.y, ecefOrigin.z);
    }
    else {
        // Resend request if timer expires
        if (Timer_isExpired(TIMER_SETORIGIN)) {
            // Resend request origin if timed out
            if (resendMessageCount >= RESEND_MESSAGE_LIMIT) {
                // Sent too many times
                setError(ERROR_NO_ORIGIN);
                return;
            }
            else {
                DBPRINTF("Resending origin request.\n");
                Mavlink_sendRequestOrigin(NO_ACK); // just want message
                Timer_new(TIMER_SETORIGIN, RESEND_MESSAGE_DELAY);
                resendMessageCount++;
            }
        } // timer expired
    }
}
Beispiel #9
0
Datei: Atlas.c Projekt: ddeo/sdp
/**********************************************************************
 * Function: doStationKeepSM
 * @return None
 * @remark Steps into the station keeping state machine.
 **********************************************************************/
static void doStationKeepSM() {
    switch (subState) {
        case STATE_STATIONKEEP_RETURN:
            // Driving to the station
            if (event.flags.navigationDone) {
                subState = STATE_STATIONKEEP_IDLE;
                Timer_new(TIMER_STATIONKEEP,STATION_KEEP_DELAY);
                DBPRINT("Arrived at station.\n");
            }
            // TODO obstacle detection
            break;
        case STATE_STATIONKEEP_IDLE:
            // Wait to float away from the station
            if (Timer_isExpired(TIMER_STATIONKEEP)) {
                // Check if we floated too far away from the station
                if (Navigation_getLocalDistance(&nedStation) > STATION_TOLERANCE_MAX) {
                    startStationKeepSM(); // return to station
                    return;
                }

                Timer_new(TIMER_STATIONKEEP, STATION_KEEP_DELAY);
            }
            break;
    }
    if (event.flags.navigationError) {
        setError((error_t)Navigation_getError());
    }
}
Beispiel #10
0
static uint16_t readDevice(uint8_t deviceReadAddress, uint8_t deviceWriteAddress, uint8_t dataAddress) {
    char success = FALSE;
    uint16_t data = 0;

    do {
        // Send the start bit with the restart flag low
        if(!I2C_startTransfer(ENCODER_I2C_ID, I2C_WRITE )){
            DBPRINT("Encoder: FAILED initial transfer!\n");
            break;
        }
        // Transmit the slave's address to notify it
        if (!I2C_sendData(ENCODER_I2C_ID, deviceWriteAddress))
            break;

        // Tranmit the read address module
        if(!I2C_sendData(ENCODER_I2C_ID, dataAddress)){
            DBPRINT("Encoder: Error: Sent byte was not acknowledged\n");
            break;
        }

        // Send a Repeated Started condition
        if(!I2C_startTransfer(ENCODER_I2C_ID,I2C_READ)){
            DBPRINT("Encoder: FAILED Repeated start!\n");
            break;
        }
        // Transmit the address with the READ bit set
        if (!I2C_sendData(ENCODER_I2C_ID, deviceReadAddress))
            break;
        
        // Read the I2C bus twice
        data = (I2C_getData(ENCODER_I2C_ID) << 6);
        I2C_acknowledgeRead(ENCODER_I2C_ID, TRUE);
        while(!I2C_hasAcknowledged(ENCODER_I2C_ID));
        data |= (I2C_getData(ENCODER_I2C_ID) & 0x3F);

        // Send the stop bit to finish the transfer
        I2C_stopTransfer(ENCODER_I2C_ID);

        success = TRUE;
    } while(0);
    if (!success) {
        DBPRINT("Encoder: Data transfer unsuccessful.\n");
        return FALSE;
    }
    return data;
}
Beispiel #11
0
static int scull_read(struct file *filp, char __user *buf, size_t size, 
							loff_t *f_off)
{
	struct scull_dev *tmp1;
	int tmp2, tmp3, tmp6, tmp7, tmp8, result;
	struct quantum *tmp4;
	void *tmp5;
	tmp1 = filp->private_data;
	tmp2 = *f_off / QUANTUM_SIZE;
	tmp3 = *f_off % QUANTUM_SIZE;
	DBPRINTI(size);
	DBPRINTI((int)*f_off);
	if (down_interruptible(&tmp1->m))
		return -ERESTARTSYS;
	while (*f_off >= tmp1->sum_len) {
		DEFINE_WAIT(tmp9);
		DBPRINT("Wait queue");
		up(&tmp1->m);
		if (filp->f_flags == O_NONBLOCK)
			return -EAGAIN;
		prepare_to_wait_exclusive(&tmp1->q, &tmp9, TASK_INTERRUPTIBLE);
		DBPRINT("waiting");
		if (*f_off >= tmp1->sum_len)
			schedule();
		finish_wait(&tmp1->q, &tmp9);
		if (signal_pending(current))
			return -ERESTARTSYS;
		if (down_interruptible(&tmp1->m))
			return -ERESTARTSYS;
	}
	tmp4 = follow_quantums(&tmp1->head, tmp2);
	tmp5 = tmp4->data;
	tmp5 += tmp3;
	tmp6 = tmp4->len - tmp3;
	tmp8 = size > tmp6 ? tmp6 : size;
	tmp7 = copy_to_user(buf, tmp5, tmp8);
	result = tmp6 - tmp7;
	*f_off += result;
	DBPRINTI(tmp8);
	up(&tmp1->m);
	return result;
}
Beispiel #12
0
Datei: Atlas.c Projekt: ddeo/sdp
/**********************************************************************
 * Function: doGpsCorrectionUpdate
 * @return None.
 * @remark Receives GPS correction data and applies it to the navigation
 *  module, or turns it off if no new message were received.
 * @author David Goodman
 * @date 2013.05.05
 **********************************************************************/
static void gpsCorrectionUpdate() {
    if (event.flags.haveGeocentricErrorMessage) {
        GeocentricCoordinate ecefError;
        ecefError.x = Mavlink_newMessage.gpsGeocentricData.x;
        ecefError.y = Mavlink_newMessage.gpsGeocentricData.y;
        ecefError.z = Mavlink_newMessage.gpsGeocentricData.z;
        Navigation_setGeocentricError(&ecefError);
        if (!Navigation_isUsingErrorCorrection())
            DBPRINT("Error corrections enabled.\n");

        Navigation_enableErrorCorrection();

        Timer_new(TIMER_GPS_CORRECTION_LOST, GPS_CORRECTION_LOST_DELAY);
    }
    else if (Timer_isExpired(TIMER_GPS_CORRECTION_LOST)) {
        // Disable error corrections
        Navigation_disableErrorCorrection();
        DBPRINT("Error corrections disabled due to telemetry timeout.\n");
    }
}
Beispiel #13
0
Datei: Atlas.c Projekt: ddeo/sdp
/**********************************************************************
 * Function: startSetOriginSM
 * @return None.
 * @remark Transitions into the set origin state machine.
 * @author David Goodman
 * @date 2013.05.04  
 **********************************************************************/
static void startSetOriginSM() {
    state = STATE_SETORIGIN;
    subState = STATE_NONE;

    // Send status message to command center to request origin coordinate
    Mavlink_sendStatus(MAVLINK_REQUEST_ORIGIN);

    resendMessageCount = 0;
    Timer_new(TIMER_SETORIGIN, RESEND_MESSAGE_DELAY);
    DBPRINT("Requesting origin.\n");
}
Beispiel #14
0
Datei: Atlas.c Projekt: ddeo/sdp
/**********************************************************************
 * Function: startStationKeepSM
 * @return None.
 * @remark Transitions into the station keeping state machine.
 * @author David Goodman
 * @date 2013.04.26  
 **********************************************************************/
static void startStationKeepSM() {
    state = STATE_STATIONKEEP;
    subState = STATE_STATIONKEEP_RETURN;

    // Send status message for debugging
    Mavlink_sendStatus(MAVLINK_STATUS_RETURN_STATION);

    Navigation_gotoLocalCoordinate(&nedStation, STATION_TOLERANCE_MIN);

    DBPRINT("Headed to station.\n");
}
Beispiel #15
0
/*
 *	Allocate a multicast cache entry
 */
static struct mfc_cache *ipmr_cache_alloc(void)
{
 /* Santosh: replaced GFP_KERNEL with GFP_ATOMIC, b'cos kmem_cache_alloc crashes @ line 1138 of slab.c */
#if defined (CONFIG_IFX_IGMP_PROXY) || defined (CONFIG_IFX_IGMP_PROXY_MODULE)
 
	struct mfc_cache *c=kmem_cache_alloc(mrt_cachep, GFP_ATOMIC);

#else

	struct mfc_cache *c=kmem_cache_alloc(mrt_cachep, GFP_KERNEL);
#endif
	if(c==NULL)
	{
		DBPRINT (" ipmr_cache_alloc, malloc failed, return NULL\n");
		return NULL;
	}
	DBPRINT (" ipmr_cache_alloc, alloc done, return\n");
	memset(c, 0, sizeof(*c));
	c->mfc_un.res.minvif = MAXVIFS;
	return c;
}
Beispiel #16
0
Datei: Atlas.c Projekt: ddeo/sdp
/**********************************************************************
 * Function: startOverrideSM
 * @return None.
 * @remark Transitions into the override state machine.
 * @author David Goodman
 * @date 2013.04.26  
 **********************************************************************/
static void startOverrideSM() {
    state = STATE_OVERRIDE;
    subState = STATE_NONE;

    Override_giveReceiverControl();
    DBPRINT("Reciever has control.\n");
    Navigation_cancel();

    #ifdef USE_SIREN
    Siren_blueLightOn();
    #endif
}
Beispiel #17
0
static void ipmr_destroy_unres(struct mfc_cache *c)
{
	struct sk_buff *skb;

	atomic_dec(&cache_resolve_queue_len);

	DBPRINT ("ipmr_destroy_unres - group: %u.%u.%u.%u \n", NIPQUAD(c->mfc_mcastgrp));
	
	while((skb=skb_dequeue(&c->mfc_un.unres.unresolved))) {
		if (skb->nh.iph->version == 0) {
			struct nlmsghdr *nlh = (struct nlmsghdr *)skb_pull(skb, sizeof(struct iphdr));
			nlh->nlmsg_type = NLMSG_ERROR;
			nlh->nlmsg_len = NLMSG_LENGTH(sizeof(struct nlmsgerr));
			skb_trim(skb, nlh->nlmsg_len);
			((struct nlmsgerr*)NLMSG_DATA(nlh))->error = -ETIMEDOUT;
			netlink_unicast(rtnl, skb, NETLINK_CB(skb).dst_pid, MSG_DONTWAIT);
		} else
			kfree_skb(skb);
	}
	DBPRINT ("ipmr_destroy_unres - calling kmem_cache_free, group: %u.%u.%u.%u \n", NIPQUAD(c->mfc_mcastgrp));
	kmem_cache_free(mrt_cachep, c);
}
Beispiel #18
0
/**********************************************************************
 * Function: setRightMotor
 * @param Speed in percent from 0 to 100%.
 * @return None
 * @remark Sets the right motor to drive at the given speed in percent.
 **********************************************************************/
static void setRightMotor(uint8_t speed) {
    uint16_t rc_time = MOTOR_PERCENT_TO_RCPULSE(speed);
    // Limit the rc times
    if (rc_time > RC_MOTOR_MAX)
        rc_time = RC_MOTOR_MAX;
    if (rc_time < RC_MOTOR_MIN)
        rc_time = RC_MOTOR_MIN;

    DBPRINT("Setting right motor to RC_TIME=%d\n",rc_time);
    #ifndef DISABLE_MOTORS
    RC_setPulseTime(MOTOR_RIGHT, rc_time);
    #endif
}
Beispiel #19
0
/**********************************************************************
 * Function: setRudder
 * @param Direction left or right to set the rudder to (0 = LEFT, 1 = RIGHT).
 * @param Angle in percent of full rudder range to set to (0 to 100%).
 * @return None
 * @remark Sets the right motor to drive at the given speed in percent.
 **********************************************************************/
static void setRudder(char direction, uint8_t percentAngle) {
    uint16_t rc_time = (direction == RUDDER_TURN_LEFT)?
        RUDDER_PERCENT_TO_RCPULSE_LEFT(percentAngle)
        :
        RUDDER_PERCENT_TO_RCPULSE_RIGHT(percentAngle);

    // Limit the rc times
    if (rc_time > RC_RUDDER_MAX)
        rc_time = RC_RUDDER_MAX;
    if (rc_time < RC_RUDDER_MIN)
        rc_time = RC_RUDDER_MIN;

    DBPRINT("Setting rudder to RC_TIME=%d\n",rc_time);
    RC_setPulseTime(RUDDER, rc_time);
}
Beispiel #20
0
/*
 *  Entry point
 */
int main(void) {

    Watchdog_init();
    DBPRINT("Watchdog initialized.\n");

    while (1) {
        doWatchdog();

        #ifdef USE_XBEE
        Xbee_runSM();
        #endif
    }

    return (EXIT_SUCCESS);
}
Beispiel #21
0
static int scull_init(void)
{
	int i;
	dev_t tmp1;
	struct scull_dev *tmp2;

	DBPRINT("new scull\n");
	alloc_chrdev_region(&tmp1, 0, DEV_COUNT, "scull");
	scull.major = MAJOR(tmp1);
	for (i = 0; i < DEV_COUNT; i++) {
		tmp1 = MKDEV(scull.major, i);
		tmp2 = scull.devs + i;
		scull_dev_init(tmp2, tmp1);
	}
	proc_create("scull", 0, NULL, &scull_seq_fops);
	return 0;
}
Beispiel #22
0
/*
 * This function replaces sys/dev/cninit.c
 * Determine which device is the console using
 * the PROM "input source" and "output sink".
 */
void
consinit()
{
	register int chosen;
	char buffer[128];
	extern int stdinnode, fbnode;
	char *consname = "unknown";
	
	DBPRINT(("consinit()\r\n"));
	if (cn_tab != &consdev_prom) return;
	
	DBPRINT(("setting up stdin\r\n"));
	chosen = OF_finddevice("/chosen");
	OF_getprop(chosen, "stdin",  &stdin, sizeof(stdin));
	DBPRINT(("stdin instance = %x\r\n", stdin));
	
	if ((stdinnode = OF_instance_to_package(stdin)) == 0) {
		printf("WARNING: no PROM stdin\n");
	}
#if NUKBD > 0
	else {
		if (OF_getprop(stdinnode, "compatible", buffer,
		    sizeof(buffer)) != -1 && strncmp("usb", buffer, 3) == 0)
			ukbd_cnattach();
	}
#endif

	DBPRINT(("setting up stdout\r\n"));
	OF_getprop(chosen, "stdout", &stdout, sizeof(stdout));
	
	DBPRINT(("stdout instance = %x\r\n", stdout));
	
	if ((fbnode = OF_instance_to_package(stdout)) == 0)
		printf("WARNING: no PROM stdout\n");
	
	DBPRINT(("stdout package = %x\r\n", fbnode));
	
	if (stdinnode && (OF_getproplen(stdinnode,"keyboard") >= 0)) {
		consname = "keyboard/display";
	} else if (fbnode && 
		   (OF_instance_to_path(stdin, buffer, sizeof(buffer)) >= 0)) {
		consname = buffer;
	}
	printf("console is %s\n", consname);
 
	/* Initialize PROM console */
	(*cn_tab->cn_probe)(cn_tab);
	(*cn_tab->cn_init)(cn_tab);
}
Beispiel #23
0
Datei: Atlas.c Projekt: ddeo/sdp
/**********************************************************************
 * Function: startRescueSM
 * @return None.
 * @remark Transitions into the rescue state machine.
 * @author David Goodman
 * @date 2013.04.26  
 **********************************************************************/
static void startRescueSM() {
    state = STATE_RESCUE;
    subState = STATE_RESCUE_GOTO;


    // Get location data from message
    nedRescue.north = Mavlink_newMessage.gpsLocalData.north;
    nedRescue.east = Mavlink_newMessage.gpsLocalData.east;
    nedRescue.down = Mavlink_newMessage.gpsLocalData.down;

    // Send status message for debugging
    Mavlink_sendStatus(MAVLINK_STATUS_START_RESCUE);
    Navigation_gotoLocalCoordinate(&nedStation, RESCUE_TOLERANCE);

    #ifdef USE_SIREN
    Siren_redLightOn();
    #endif

    DBPRINT("Rescuing person at: N=%.2f, E=%.2f, D=%.2f.\n",
        nedRescue.north, nedRescue.east, nedRescue.down);
}
Beispiel #24
0
static void ipmr_update_threshoulds(struct mfc_cache *cache, unsigned char *ttls)
{
	int vifi;

	cache->mfc_un.res.minvif = MAXVIFS;
	cache->mfc_un.res.maxvif = 0;
	memset(cache->mfc_un.res.ttls, 255, MAXVIFS);

	for (vifi=0; vifi<maxvif; vifi++) {
		if (VIF_EXISTS(vifi) && ttls[vifi] && ttls[vifi] < 255) {
			cache->mfc_un.res.ttls[vifi] = ttls[vifi];
			if (cache->mfc_un.res.minvif > vifi)
				cache->mfc_un.res.minvif = vifi;
			if (cache->mfc_un.res.maxvif <= vifi)
				cache->mfc_un.res.maxvif = vifi + 1;
		}
	}

	DBPRINT ("ipmr_update_threshoulds - cache->mfc_un.res.maxvif: %d, cache->mfc_un.res.minvif: %d\n", 
		cache->mfc_un.res.maxvif, cache->mfc_un.res.minvif);

}
Beispiel #25
0
Datei: Atlas.c Projekt: ddeo/sdp
/**********************************************************************
 * Function: doRescueSM
 * @return None
 * @remark Executes one cycle of the boat's rescue state machine.
 **********************************************************************/
static void doRescueSM() {
    switch (subState) {
        case STATE_RESCUE_GOTO:
            if (event.flags.navigationDone) {
                subState = STATE_RESCUE_SEARCH;
                Navigation_cancel();
            }
            break;
        case STATE_RESCUE_SEARCH:
            // Human sensor event handling
            // Falls through to success for now
            //if 
            subState = STATE_RESCUE_SUPPORT;
            DBPRINT("Rescue mission complete.\n");
            Navigation_cancel();
            Mavlink_sendStatus(MAVLINK_STATUS_RESCUE_SUCCESS);
            break;
        case STATE_RESCUE_SUPPORT:
            // Do nothing
            break;
    }
    
}
Beispiel #26
0
void GPWithRankPrior::copySettings(const GPWithRankPrior & model)
{
	//TODO  Need Update One Data Changed
	// Make sure number of data points is equal.
    assert(model.mX.rows() == mX.rows());

    // Copy parameters that don't require special processing.
    this->mSequence = model.mSequence;
    this->mDataMatrix = model.mDataMatrix;
    this->mY = model.mY;

    // Copy latent positions or convert them to desired dimensionality.
    if (model.mX.cols() == mX.cols())
    {
        mX = model.mX;
    }
    else
    {
 	         // Pull out the largest singular values to keep.
        JacobiSVD<MatrixXd> svd(model.mX, ComputeThinU | ComputeThinV);

		VectorXd S = svd.singularValues();
        this->mX = svd.matrixU().block(0,0,this->mX.rows(),this->mX.cols())*S.head(this->mX.cols()).asDiagonal();

		std::cout << mX << std::endl;
        // Report on the singular values that are kept and discarded. 
        DBPRINTLN("Largest singular value discarded: " << S(this->mX.cols()));
        DBPRINTLN("Smallest singular value kept: " << S(this->mX.cols()-1));
        DBPRINTLN("Average singular value kept: " << ((1.0/((double)this->mX.cols()))*S.head(this->mX.cols()).sum()));
        DBPRINT("Singular values: ");
        DBPRINTMAT(S); 
    }
 
	if (mReconstructionGP)  mReconstructionGP->copySettings(model.mReconstructionGP);
	if (mBackConstraint)    mBackConstraint->copySettings(model.mBackConstraint);
}
Beispiel #27
0
/**********************************************************************
 * Function: updateHeading
 * @return None
 * @remark Determines the heading error using the tilt-compensated compass,
 *  and adjusts the rudder accordingly. Also, a bang-bang control has been
 *  implemented to turn the rudder to the maximum value if the boat's motors
 *  are being driven below some percentage defined above.
 * @author Darrel Deo
 * @author David Goodman
 * @date 2013.03.27 
 **********************************************************************/
static void updateRudder() {

    static int16_t lastThetaError; // used for derivative term
    
    rudderDirection = RUDDER_TURN_LEFT;
    
    // Get current heading, determine theta error
    uint16_t currentHeading = (uint16_t)TiltCompass_getHeading();
    int16_t thetaError = desiredHeading - currentHeading;
    
    // Bound theta error and determine turn direction
    if (thetaError > 0) {
        rudderDirection = (thetaError < 180)? RUDDER_TURN_RIGHT : RUDDER_TURN_LEFT;
        thetaError = (thetaError < 180)? thetaError : (360 - thetaError);
    }
    else {
        // theta error is negative
        rudderDirection = (thetaError > -180)? RUDDER_TURN_LEFT : RUDDER_TURN_RIGHT;
        thetaError = (thetaError > -180)? -thetaError : (360 + thetaError);
    }
    
    // Initialize or dump derivative if changed directions
    if (lastRudderDirection == RUDDER_TURN_NONE || rudderDirection != lastRudderDirection)
        lastThetaError = 0;
        
    /*    Controller Terms    */
    // Derivative (scaled by KD_RUDDER)
    float thetaErrorDerivative = (float)abs(thetaError - 
        lastThetaError)/MS_TO_SEC(TRACK_UPDATE_DELAY);
    
    // Proportional (scaled by KP_RUDDER), convert degrees to percent
    float uDegrees = KP_RUDDER*(float)thetaError + KD_RUDDER*thetaErrorDerivative;
    float uPercent = (uDegrees / RUDDER_ANGLE_MAX ) * 100;
    
    // Limit percentage from 0 to 100
    uPercent = (uPercent > 100.0)? 100.0f : uPercent;
    uPercent = (uPercent < 0.0)? 0.0f : uPercent;

    char *bangbang = "";
    // Bang-bang control to force rudder all the way if speed is low
    if (desiredSpeed < RUDDER_BANGBANG_SPEED_THRESHOLD
            && thetaError > RUDDER_BANGBANG_THETA_DEADBAND_THRESHOLD) {
        uPercent = 100.0f;
        bangbang = " BB";
    }
    
    // Command the rudder and save 
    setRudder(rudderDirection, (uint8_t)uPercent);
    lastThetaError = thetaError;
    lastRudderDirection = rudderDirection;
    char *dir;
    dir = (rudderDirection == RUDDER_TURN_RIGHT)? "R" : "L";
        
    #ifdef DEBUG_VERBOSE
    DBPRINT("Rudder control: rDegrees=%d, yDegrees=%d, eDegrees=%d, uDegrees=%.2f, uPercent=%d[%s]\n\n",
        desiredHeading, currentHeading, thetaError, uDegrees, (uint8_t)uPercent, dir);
    #endif

    #ifdef USE_PUBLIC_DEBUG
    sprintf(debugString, "R=%d, Y=%d, e=%d, U=%.2f, Up=%d[%s]%s, S=%d\n",
        desiredHeading, currentHeading, thetaError, uDegrees, (uint8_t)uPercent, dir, bangbang, desiredSpeed);
    #endif
}
Beispiel #28
0
Datei: Atlas.c Projekt: ddeo/sdp
/**********************************************************************
 * Function: setError
 * @param Error code to set current error to.
 * @return None.
 * @remark Sets an error that occurred.
 * @author David Goodman
 * @date 2013.05.04
 **********************************************************************/
static void setError(error_t errorCode) {
    lastErrorCode = errorCode;
    event.flags.haveError = TRUE;
    Mavlink_sendError(errorCode);
    DBPRINT("Error: %s\n", getErrorMessage(errorCode));
}
Beispiel #29
0
Datei: Atlas.c Projekt: ddeo/sdp
/**********************************************************************
 * Function: checkEvents
 * @return None
 * @remark Checks for various events that can occur, and sets the 
 *  appropriate flags for handling later.
 **********************************************************************/
static void checkEvents() {
    // Clear all event flags
    int i;
    for (i=0; i < EVENT_BYTE_SIZE; i++)
        event.bytes[i] = 0x0;       


    // Navigation events
    if (Navigation_isDone())
        event.flags.navigationDone = TRUE;

    if (Navigation_hasError())
        setError(Navigation_getError());

    // Override
    if (Override_isTriggered())
        event.flags.receiverDetected = TRUE;


    // XBee messages (from command center)
    if (Mavlink_hasNewMessage()) {
        lastMavlinkMessageID = Mavlink_getNewMessageID();
        lastMavlinkCommandID = MAVLINK_NO_COMMAND;
        lastMavlinkMessageWantsAck = FALSE;
        switch (lastMavlinkMessageID) {
            // -------------------------- Acknowledgements ------------------------
            case MAVLINK_MSG_ID_MAVLINK_ACK:
                // No ACKS from ComPAS to AtLAs
                break;
            
            // ------------------------------ Messages ----------------------------
            case MAVLINK_MSG_ID_CMD_OTHER:
                lastMavlinkMessageWantsAck = Mavlink_newMessage.commandOtherData.ack;
                if (Mavlink_newMessage.commandOtherData.command == MAVLINK_RESET_BOAT ) {
                    event.flags.haveResetMessage = TRUE;
                    lastMavlinkCommandID = MAVLINK_RESET_BOAT ;
                }
                else if (Mavlink_newMessage.commandOtherData.command == MAVLINK_RETURN_STATION) {
                    event.flags.haveReturnStationMessage = TRUE;
                    overrideShutdown = FALSE;
                    lastMavlinkCommandID = MAVLINK_RETURN_STATION;
                }
                else if (Mavlink_newMessage.commandOtherData.command == MAVLINK_SAVE_STATION) {
                    event.flags.haveSetStationMessage = TRUE;
                    wantSaveStation = TRUE;
                    lastMavlinkCommandID = MAVLINK_SAVE_STATION;
                }
                else if (Mavlink_newMessage.commandOtherData.command == MAVLINK_GEOCENTRIC_ORIGIN) {
                    event.flags.haveSetOriginMessage = TRUE;
                    lastMavlinkCommandID = MAVLINK_GEOCENTRIC_ORIGIN;
                }
                else if (Mavlink_newMessage.commandOtherData.command == MAVLINK_OVERRIDE) {
                    event.flags.haveOverrideMessage = TRUE;
                    overrideShutdown = TRUE;
                    lastMavlinkCommandID = MAVLINK_OVERRIDE;
                }
                break;
            case MAVLINK_MSG_ID_GPS_ECEF:
                lastMavlinkMessageWantsAck = Mavlink_newMessage.gpsGeocentricData.ack;
                if (Mavlink_newMessage.gpsGeocentricData.status == MAVLINK_GEOCENTRIC_ORIGIN) {
                    event.flags.haveSetOriginMessage = TRUE;
                    lastMavlinkCommandID = MAVLINK_GEOCENTRIC_ORIGIN;
                }
                else if (Mavlink_newMessage.gpsGeocentricData.status == MAVLINK_GEOCENTRIC_ERROR)
                    event.flags.haveGeocentricErrorMessage = TRUE;
                break;
            case MAVLINK_MSG_ID_GPS_NED:
                lastMavlinkMessageWantsAck = Mavlink_newMessage.gpsLocalData.ack;
                if (Mavlink_newMessage.gpsLocalData.status == MAVLINK_LOCAL_SET_STATION) {
                    event.flags.haveSetStationMessage = TRUE;
                    lastMavlinkCommandID = MAVLINK_LOCAL_SET_STATION;
                    wantSaveStation = FALSE;
                }
                else if (Mavlink_newMessage.gpsLocalData.status == MAVLINK_LOCAL_START_RESCUE) {
                    event.flags.haveStartRescueMessage = TRUE;
                    lastMavlinkCommandID = MAVLINK_LOCAL_START_RESCUE;
                }
                break;
            default:
                // Unknown message ID
                event.flags.haveUnknownMessage = TRUE;
                DBPRINT("Mavlink received unhandled message: 0x%X\n");
                break;
        }
    }
    /* If the receiver is off, boat is not stopped, and we either
        have a station or are setting one, then turn override off */
    if (!event.flags.receiverDetected && !overrideShutdown 
        && (haveStation || (state == STATE_SETSTATION))) {

        event.flags.wantOverride = FALSE;
    }
    else {
        event.flags.wantOverride = TRUE;
    }
} //  checkEvents()
Beispiel #30
0
Datei: Atlas.c Projekt: ddeo/sdp
/**********************************************************************
 * Function: doMasterSM
 * @return None.
 * @remark Executes one cycle of the boat's master state machine.
 * @author David Goodman
 * @date 2013.03.28 
 **********************************************************************/
static void doMasterSM() {
    checkEvents();

    #ifdef USE_TILTCOMPASS
    TiltCompass_runSM();
    #endif

    #ifdef USE_GPS
    GPS_runSM();
    #endif

    #ifdef USE_NAVIGATION
    Navigation_runSM();
    #ifdef USE_ERROR_CORRECTION
    gpsCorrectionUpdate();
    #endif
    #endif

    #ifdef USE_DRIVE
    Drive_runSM();
    #endif

    #ifdef USE_XBEE
    Xbee_runSM();
    #endif

    #ifdef USE_BAROMETER
    Barometer_runSM();
    doBarometerUpdate(); // send barometer data
    #endif

    switch (state) {
        case STATE_SETSTATION:
            doSetStationSM();

            if (event.flags.haveStartRescueMessage) {
                startRescueSM();
            }
            else if (event.flags.setStationDone) {
                if (haveOrigin)
                    startStationKeepSM();
                else
                    startOverrideSM();   
            }
            
            break;
        case STATE_SETORIGIN:
            doSetOriginSM();
            
            if (event.flags.setOriginDone)
                startOverrideSM();   
            
            break;
        case STATE_STATIONKEEP:
            doStationKeepSM();

            if (event.flags.haveStartRescueMessage)
                startRescueSM();
            else if (!haveStation)
                setError(ERROR_NO_STATION);
            break;

        case STATE_OVERRIDE:
            if (!wantOverride) {
                    //setError(ERROR_NO_ORIGIN);
                if (!haveOrigin)
                    startSetOriginSM(); // do we ant infinite startup loop?
                else if (event.flags.haveStartRescueMessage)
                    startRescueSM();
                else if (event.flags.haveSetStationMessage )
                    startSetStationSM();
                else if (haveOrigin && haveStation)
                    startStationKeepSM();
                
                // Use autonomous controls
                if (haveOrigin && (haveStation
                    || event.flags.haveStartRescueMessage)) {
                    Override_giveMicroControl();
                    DBPRINT("Micro has control.\n");
                    #ifdef USE_SIREN
                    Siren_blueLightOff();
                    #endif
                }
            }

            break;

        case STATE_RESCUE:
            doRescueSM();

            
            if (event.flags.haveStartRescueMessage) {
                startRescueSM();
            }
            else if (event.flags.haveReturnStationMessage) {
                if (haveStation)
                    startStationKeepSM();
                else 
                    setError(ERROR_NO_STATION);
            }
            // Turn off rescue siren (red)
            if (event.flags.haveError || state != STATE_RESCUE) {
                #ifdef USE_SIREN
                Siren_redLightOff();
                #endif
            }

            break;
    }
    //  ------- Caught by most states -----------
    if (state != STATE_RESCUE) {
        if (event.flags.haveSetStationMessage)
            startSetStationSM();
    }
    if (state != STATE_OVERRIDE) {
        if (event.flags.haveError) {
            startOverrideSM();
            overrideShutdown = TRUE;
        }
        if (wantOverride)
            startOverrideSM();
    }
    if (event.flags.haveResetMessage)
        resetAtlas();
}