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