void Rover::set_mode(enum mode mode)
{
if(control_mode == mode){
// don't switch modes if we are already in the correct mode.
return;
}
// If we are changing out of AUTO mode reset the loiter timer
if (control_mode == AUTO)
loiter_time = 0;
control_mode = mode;
throttle_last = 0;
throttle = 500;
set_reverse(false);
g.pidSpeedThrottle.reset_I();
if (control_mode != AUTO) {
auto_triggered = false;
}
switch(control_mode)
{
case MANUAL:
case HOLD:
case LEARNING:
case STEERING:
break;
case AUTO:
rtl_complete = false;
restart_nav();
break;
case RTL:
do_RTL();
break;
case GUIDED:
rtl_complete = false;
/*
when entering guided mode we set the target as the current
location. This matches the behaviour of the copter code.
*/
guided_WP = current_loc;
set_guided_WP();
break;
default:
do_RTL();
break;
}
if (should_log(MASK_LOG_MODE)) {
DataFlash.Log_Write_Mode(control_mode);
}
}
void Rover::set_mode(enum mode mode)
{
if(control_mode == mode){
// don't switch modes if we are already in the correct mode.
return;
}
control_mode = mode;
throttle_last = 0;
throttle = 500;
set_reverse(false);
g.pidSpeedThrottle.reset_I();
if (control_mode != AUTO) {
auto_triggered = false;
}
switch(control_mode)
{
case MANUAL:
case HOLD:
case LEARNING:
case STEERING:
break;
case AUTO:
rtl_complete = false;
restart_nav();
break;
case RTL:
do_RTL();
break;
case GUIDED:
rtl_complete = false;
set_guided_WP();
break;
default:
do_RTL();
break;
}
if (should_log(MASK_LOG_MODE)) {
DataFlash.Log_Write_Mode(control_mode);
}
}
static portTASK_FUNCTION( vConductorUpdateTask, pvParameters )
{
uint8_t rxLen, status;
uint8_t Buffer[vtI2CMLen];
uint8_t *valPtr = &(Buffer[0]);
// Get the parameters
vtConductorStruct *param = (vtConductorStruct *) pvParameters;
// Get the I2C device pointer
vtI2CStruct *devPtr = param->dev;
// Get the LCD information pointer
vtTempStruct *tempData = param->tempData;
myNavStruct *navData = param->navData;
myMapStruct *mapData = param->mapData;
uint8_t recvMsgType;
mapStruct.mappingFlag = 0;
uint8_t i2cRoverMoveStop[] = {0x05, 0x00};
// 255 will always be a bad message
countDefArray[255] = BadMsg;
uint8_t i;
for(i = 0; i < 255; i++) {
countDefArray[i] = CleanMsg;
}
uint8_t i2cRoverMoveStop[] = {0x05, 0x00};
// Like all good tasks, this should never exit
for(;;)
{
// Wait for a message from an I2C operation
if (vtI2CDeQ(devPtr,vtI2CMLen,Buffer,&rxLen,&recvMsgType,&status) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
// Decide where to send the message
// This just shows going to one task/queue, but you could easily send to
// other Q/tasks for other message types
// This isn't a state machine, it is just acting as a router for messages
// If it is the initialization message
if ( recvMsgType == vtI2CMsgTypeTempInit ) {
SendTempValueMsg(tempData,recvMsgType,Buffer,portMAX_DELAY);
}
else {
// Switch on the definition of the incoming count
switch(countDefArray[Buffer[0]]) {
case RoverMsgSensorAllData: {
//printf("SensorData\n");
GPIO_ClearValue(0,0x78000);
GPIO_SetValue(0, 0x48000);
SendMapValueMsg(mapData,RoverMsgSensorAllData, Buffer, portMAX_DELAY);
if (!speedRun)
SendNavValueMsg(navData,0x11,Buffer,portMAX_DELAY);
break;
}
case RoverMsgSensorForwardRight: {
//SendTempValueMsg(tempData,recvMsgType,Buffer,portMAX_DELAY);
break;
}
case RoverMsgMotorLeftData: {
//printf("MotorLeftData\n");
GPIO_ClearValue(0,0x78000);
GPIO_SetValue(0, 0x40000);
SendTempValueMsg(tempData,RoverMsgMotorLeftData,Buffer,portMAX_DELAY);
if (!speedRun)
SendNavValueMsg(navData,RoverMsgMotorLeftData,Buffer,portMAX_DELAY);
SendMapValueMsg(mapData,RoverMsgMotorLeftData,Buffer,portMAX_DELAY);
break;
}
case RoverMsgMotorLeft90: {
printf("RoverData\n");
break;
// SendTempValueMsg(tempData,recvMsgType,Buffer,portMAX_DELAY);
}
case BadMsg: {
printf("Bad Message; Restart Pics\n");
break;
}
default: {
//printf("ConductDefault\n");
SendMapValueMsg(mapData,0x11,Buffer,portMAX_DELAY);
break;
}
}
if(Buffer[6] == 1) {
if (vtI2CEnQ(devPtr,vtI2CMsgTypeTempRead1,0x4F,sizeof(i2cRoverMoveStop),i2cRoverMoveStop,10) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(0);
}
mapStruct.mappingFlag = 0;
mapStruct.timerFlag = 0;
restart_nav();
printf("\nFINISH LINE DETECTED!\n");
stopGettingMotor("DATASTOP");
}
}
// Clear the count defition
countDefArray[Buffer[0]] = CleanMsg;
// Check if retransmission is needed
if ( countDefArray[Buffer[0]-3] == RoverMsgMotorLeft90 ) {
printf("Retrans of motor\n");
SendNavValueMsg(navData,0x89,Buffer,portMAX_DELAY);
}
// Check to see if the last five have been retrieved
/*uint8_t i;
for (i = Buffer[0]-1; i > Buffer[0] - 5; i--) {
printf("Buf Check: %d\n", i);
if ( countDefArray[i] == RoverMsgMotorLeft90 ) {
printf("Retrans of motor\n");
SendNavValueMsg(navData,0x89,Buffer,portMAX_DELAY);
}
}*/
}
}
