/**********************************************************************
* @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);
}
}
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);
}
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;
}
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;
}
/*
* 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);
}
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);
}
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);
}
}
}
/**********************************************************************
* 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
}
}
/**********************************************************************
* 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());
}
}
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;
}
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;
}
/**********************************************************************
* 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");
}
}
/**********************************************************************
* 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");
}
/**********************************************************************
* 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");
}
/*
* 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;
}
/**********************************************************************
* 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
}
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);
}
/**********************************************************************
* 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
}
/**********************************************************************
* 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);
}
/*
* Entry point
*/
int main(void) {
Watchdog_init();
DBPRINT("Watchdog initialized.\n");
while (1) {
doWatchdog();
#ifdef USE_XBEE
Xbee_runSM();
#endif
}
return (EXIT_SUCCESS);
}
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;
}
/*
* 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);
}
/**********************************************************************
* 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);
}
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);
}
/**********************************************************************
* 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;
}
}
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);
}
/**********************************************************************
* 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
}
/**********************************************************************
* 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));
}
/**********************************************************************
* 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()
/**********************************************************************
* 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();
}