void main(void) {
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
initSensors();
P1DIR |= BIT0; // Set P1.0 as output for left LED
P1DIR |= BIT6; // Set P1.6 as output for right LED
while (1)
{
// Turn on left LED on if after ADC, left sensor returns a value greater than 2FE, and if not, turn LED off
if (getLeftSensor() < 0x3EA)
{
P1OUT &= ~BIT0; // turn left LED off
}
else
{
P1OUT |= BIT0; // turn left LED on
}
// Turn on right LED if after ADC, right sensor returns a value greater than 1FF, and if not, turn LED off
if (getRightSensor() < 0x0E8)
{
P1OUT &= ~BIT6; // turn right LED off
}
else
{
P1OUT |= BIT6; // turn right LED on
}
}
}
void noreturn main(void)
{
init();
initSensors();
for(;;)
{
while(!loopActive)
{
; // wait for next loop
}
readSensors();
attitudeCalculation();
control();
actuate();
triggerSonar();
input();
output();
leds();
ATOMIC_BLOCK(ATOMIC_FORCEON)
{
lastLoopTicks = ticksSinceLoopStart;
loopActive = false;
}
}
}
void setup() {
initLeds();
initLcd();
initSensors();
enableDigitalSensors();
enableAnalogSensors();
}
void MultiWii_setup() {
SerialOpen(SERIAL0_COM_SPEED);
#if defined(TRACE)
Serial.println("Start MultiWii_setup");
#endif
initOutput();
LoadDefaults();
configureReceiver();
initSensors();
previousTime = micros();
calibratingA = 0;
calibratingG = 512;
#if defined(TRACE)
Serial.println("End MultiWii_setup");
#endif
}
int main() {
double walling = 0.0; // Control Behavior = 0.0 - center between both walls
// +500.0 = stay 50 cm from left wall
// -500.0 = stay 50 cm from right wall
int check = initSensors(&urg);
if (check != 1) {
printf("Init Error\n");
return;
}
const double tick2cm = 4.0;
int p = 0;
int s;
int ticksL;
int ticksR;
double averageDist;
//for (s = 0; s < 1;s++) {
while(1) {
int countL = 0;
int countR = 0;
p = scan_center(walling);
if (p == -1) printf("Lidar scanning error . . .\n");
if (p == 1) printf("Lost Left Wall\n");
if (p == 2) printf("Lost Right Wall\n");
if (p == 3) printf("Lost Both Walls\n");
}
printf("done\n");
return 0;
}
int main()
{
initSensors();
while(1)
readAllSensors();
return 0;
}
/*********************************
Android MAIN
**********************************/
extern void android_main(struct android_app* state) {
/////////////////////////////////
// Make sure glue isn't stripped.
app_dummy();
//enable COUT
overloadSTDCOUT();
/////////////////////////////////
//Init state
state->onAppCmd = __android_handle_cmd;
state->onInputEvent = __android_handle_input;
//save app
app_state=state;
//attach java thread
attachToAndroidThreadJava();
//init sensors
initSensors();
createSensorsEventQueue();
addAccelerometer();
////////////////////////////
int ident;
int events;
struct android_poll_source* source;
//waiting to get window
while (!getIsAndroidValidDevice()){
while ((ident = ALooper_pollAll(0, NULL, &events,(void**)&source)) >= 0){
if (source != NULL) source->process(state, source);
if (state->destroyRequested != 0) return;
}
usleep( 16 );
}
////////////////////////////
//GET APK (ZIP FILE)
ANativeActivity* activity = state->activity;
jclass clazz = (*getEnv())->GetObjectClass(getEnv(), activity->clazz);
jmethodID methodID = (*getEnv())->GetMethodID(getEnv(), clazz, "getPackageCodePath", "()Ljava/lang/String;");
jobject result = (*getEnv())->CallObjectMethod(getEnv(), activity->clazz, methodID);
jboolean isCopy;
path_apk= (const char*) (*getEnv())->GetStringUTFChars(getEnv(), (jstring)result, &isCopy);
////////////////////////////
//INIT openAL/backend
JNI_OnLoad(activity->vm,0);
//no in OpenSL ES backend
////////////////////////
//call main
char *argv[2];
argv[0] = strdup("Easy2D");
argv[1] = NULL;
int out=main(1, argv);
//call atexit
callatexitANDROID();
//deattach java thread
deattachToAndroidThreadJava();
//disable COUT
unoverloadSTDCOUT();
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
chEvtInit(&eventImuIrq);
chEvtInit(&eventMagnIrq);
chEvtInit(&eventImuRead);
chEvtInit(&eventMagnRead);
chEvtInit(&eventEKFDone);
palSetPadMode(GPIOB, 3, PAL_MODE_OUTPUT_PUSHPULL); // BLUE
palSetPadMode(GPIOB, 4, PAL_MODE_OUTPUT_PUSHPULL); // GREEN
palSetPadMode(GPIOB, 5, PAL_MODE_OUTPUT_PUSHPULL); // RED
chThdCreateStatic(waThreadLed, sizeof(waThreadLed), NORMALPRIO, ThreadLed, NULL );
I2CInitLocal();
configInit();
mavlinkInit();
initSensors();
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
TIM_TimeBaseStructure.TIM_Period = 0xFFFFFFFF;
TIM_TimeBaseStructure.TIM_Prescaler = 84 - 1;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);
TIM_Cmd(TIM5, ENABLE);
startEstimation();
startSensors();
radioInit();
motorsInit();
extStart(&EXTD1, &extcfg);
extChannelEnable(&EXTD1, 0);
extChannelEnable(&EXTD1, 1);
chEvtBroadcastFlags(&eventEKFDone, EVT_EKF_DONE);
while (TRUE) {
chThdSleepMilliseconds(1000);
}
}
void SensorsConfig::showEvent(QShowEvent *)
{
if (m_neverShown) {
initSensors();
m_neverShown = false;
}
else {
const SensorList &list = SensorBase::self()->sensorsList();
SensorList::ConstIterator it;
for (it = list.begin(); it != list.end(); ++it) {
QListViewItem *item = m_sensorView->findItem((*it).sensorName(), 1);
if (item)
item->setText(3, (*it).sensorValue() + (*it).sensorUnit());
}
}
}
Server::Server()
: tcpServer(0), networkSession(0)
{
remote_port = 51015;
local_port = 51016;
device_id = 0;
thisIPAddress = "";
remoteIPAddress = "127.0.0.1";
is_init_connection = false;
is_config_mode = false;
tcpServer = NULL;
networkSession = NULL;
remote_server_socket = NULL;
shotTimer = new QTimer(this);
shotTimer->setInterval(100000);
shotTimer->setSingleShot(false);
connect(shotTimer, SIGNAL(timeout()), this, SLOT(sendingTimeout()));
connectionTimer = new QTimer(this);
connectionTimer->setInterval(20000);
connectionTimer->setSingleShot(false);
connect(connectionTimer, SIGNAL(timeout()), this, SLOT(connectionTimeout()));
sensors = new QHash<QString, ClientSensor *>();
actions = new QHash<QString, ClientAction *>();
sensors->insert("activity", new ClientSensorActivity(this));
sensors->insert("button_activity", new ClientSensorBtnActivity(this));
//sensors->insert("mouse", new ClientSensor(this));
//sensors->insert("keyboard", new ClientSensor(this));
//sensors->insert("activity_direction", new ClientSensor(this));
//sensors->insert("internet_speed", new ClientSensor(this));
actions->insert("tone", new ClientActionTone(this));
actions->insert("melody", new ClientActionToneMelody(this));
actions->insert("lcd", new ClientActionLcd(this));
actions->insert("led", new ClientActionIndication(this));
actions->insert("led_state", new ClientActionIndicationState(this));
actions->insert("reset", new ClientActionReset(this));
actions->insert("config", new ClientActionConfig(this));
actions->insert("display_state", new ClientActionSetDisplayState(this));
//actions->insert("activity_direction", new ClientAction(this));
initSensors();
}
/**
* Runs all the initialisations that are needed
* Please put them in here.
*/
void initialise() {
debug_frmwrk_init();
setSensorSide(-1);
trackingState = 0;
timerCounter = 0;
courseType = 0;
initSerial();
serialTest();
initSensors();
// Even tho this is a test it needs to run so that the serial is set up properly
initTimers();
//__enable_irq();
if (DBG_LEVEL >= 1) {
_DBG_("MOUSE");
}
mouseinitial();
if (DBG_LEVEL >= 1) {
_DBG_("I've completed");
}
}
PitchRollHeading::PitchRollHeading() {
/* Assign a unique ID to the sensors */
dof = new Adafruit_10DOF();
accel = new Adafruit_LSM303_Accel_Unified(LSM303_ADDRESS_ACCEL);
mag = new Adafruit_LSM303_Mag_Unified(LSM303_ADDRESS_MAG);
bmp = new Adafruit_BMP085_Unified(BMP085_ADDRESS);
gyro = new Adafruit_L3GD20_Unified();
//interruptsActive = false;
// set up pin change interrupts
//gyroInt = new PCInterrupts();
//accelInt = new PCInterrupts();
//magInt = new PCInterrupts();
//imuService = new IMUInterruptService(this);
//gyroIntPin = new Digital(67); // pin 67 for now, put in configs later
//accelIntPin = new Digital(68);
//magIntPin = new Digital(52);
//gyroIntPin->pinMode(INPUT);
//accelIntPin->pinMode(INPUT);
//magIntPin->pinMode(INPUT);
//gyroInt->attachInterrupt(gyroIntPin->pin,imuService,CHANGE);
//accelInt->attachInterrupt(accelIntPin->pin,imuService,CHANGE);
//magInt->attachInterrupt(magIntPin->pin,imuService,CHANGE);
initSensors();
}
bool SkinGroup::initConfiguration(yarp::os::Property &conf) {
//int s = conf.size();
//this->groupName = conf.get(0).asString().c_str();
yarp::os::Value v = conf.find("groupname");
if(v.isNull()) {
printf("Not Group Name specified\n");
return false;
}
else {
this->groupName = v.asString().c_str();
}
initLogFile(conf);
yarp::os::Value v2 = conf.find("parts");
if(v2.isNull()) {
return false;
}
else {
// add all parts to this group
if(!initSensors(conf)) {
cerr << "ERROR: initSensors error. (SkinGroup)" << endl;
return false;
}
}
/*
for (int j = 1; j < conf.size(); j++)
{
cout << conf.get(j).asString() << endl;
int k = 0;
for (k = 0; k < parts->size(); k++)
{
if (parts->at(k)->getPartName() == string(
conf.get(j).asString().c_str()))
{
cout << "part : " << parts->at(k)->getPartName().c_str()
<< " is found and added to SkinGroup "
<< this->groupName << endl;
this->skinParts.push_back(parts->at(k));
break;
}
}
if (k == parts->size())
{
// Not found. Error
cerr << "Sensor " << conf.get(j).asString().c_str()
<< " for group " << this->groupName << " is not available."
<< endl;
return false;
}
}
// not necessary. just to double check
if (this->skinParts.size() != conf.size() - 1)
{
// Not found. Error
cerr << "Not all sensors in sensorgroup " << this->groupName
<< " are available. Check the config files" << endl;
return false;
}
*/
cout << "OK. Sensors added for sensorgroup " << this->groupName << endl;
// init other settings, e.g. offsets for each sensor in the whole data set
taxelDimension = 0;
for (size_t k = 0; k < skinParts.size(); k++)
{
offsets.push_back(taxelDimension);
taxelDimension += skinParts[k]->getAllNumTaxels();
}
// other variables
tempTactileData = new TactileData(taxelDimension);
// load svm model file
// TODO:
yarp::os::ConstString _svmmodelfile = conf.find("svmmodel").asString();
_svmmodelfile = conf.check("svmmodelfilepath", yarp::os::Value("traindata"), "").asString() + FILESEPARATOR + _svmmodelfile;
this->svmmodelfile = _svmmodelfile.c_str();
cout << "SKINGROUP : " << this->groupName << " has " << this->taxelDimension << " taxels" << endl;
//this->classification = new TactileClassificationRawData(this->taxelDimension);
this->classification = new TactileClassificationHist(this->taxelDimension);
if(!this->classification->loadSVMModel(svmmodelfile.c_str()))
{
cout << "ERROR: cannot load SVMModel file " << svmmodelfile.c_str() << endl;
return false;
}
yarp::os::Property custProp;
custProp.put("normalisation", "none");
custProp.put("hist_num_bins", 20);
classification->customConfiguration(custProp);
tState = new TactileState(TIME_DIMENSION, taxelDimension, 50);
// init port for output broadcasting
initOutport(conf);
return true;
}
/**
* init method to init Driver related Stuff
*/
void DriverInit() {
uartQsem = OSQCreate((void*) &uartQmessageTable, UART_Q_SIZE); //create Message Queue for UART driver
initRCreceiver();
initSensors();
}
void IMUReader::run()
{
initSensors();
cs::SensorData sensorMessage;
sensorMessage.set_sensor_id((int) SensorIds::IMU_sensor);
sensorMessage.set_sensor_desc(IMU_SENSOR_NAME);
auto values = sensorMessage.mutable_sensor_values();
auto gyroData = values->Add();
gyroData->set_associated_name("gyro_data");
gyroData->set_data_type(cs::COORD_3D);
auto gyroCoords = gyroData->mutable_coord_value();
int64_t loopCount = 0;
GyroState gyroState = { 0 };
bool calibration = true;
int64_t calibrationDelay = 500;
AHRSProcessor ahrsProcessor(100);
auto instance = ConfigManager::getInstance();
if (!instance)
{
COMM_EXCEPTION(NullPointerException, "Can't get instance of "
"configuration manager");
}
float pitch, roll, yaw;
QUATERNION offsetQ;
showCalibration(true);
auto delayTime = std::chrono::milliseconds(10);
auto t = std::chrono::high_resolution_clock::now();
#if WRITE_SENSORS_DATA
std::ofstream stream("raw_data.txt");
#endif
while (!stopVariable)
{
IMUSensorsData sensorsData = { 0 };
readSensors(sensorsData, gyroState, calibration);
std::this_thread::sleep_until(t + delayTime);
t += delayTime;
#if WRITE_SENSORS_DATA
stream << sensorsData.rawAccelX << " " << sensorsData.rawAccelY <<
" " << sensorsData.rawAccelZ << " " << sensorsData.rawGyroX <<
" " << sensorsData.rawGyroY << " " << sensorsData.rawGyroZ <<
std::endl;
#endif
if (!calibration)
{
ahrsProcessor.updateState(sensorsData, roll, pitch, yaw);
putCurrentAngles(roll, pitch, yaw);
}
else
{
IMUSensorsData tmpData = { 0 };
tmpData.rawAccelX = sensorsData.rawAccelX;
tmpData.rawAccelY = sensorsData.rawAccelY;
tmpData.rawAccelZ = sensorsData.rawAccelZ;
ahrsProcessor.updateState(tmpData, offsetQ);
}
gyroCoords->set_x(roll);
gyroCoords->set_y(pitch);
gyroCoords->set_z(yaw);
int messageSize = sensorMessage.ByteSize();
std::vector<int8_t> outArray(messageSize);
sensorMessage.SerializeToArray(&outArray[0], messageSize);
if (!calibration && loopCount % 10 == 0)
{
//std::cout << sensorsData.rawCompassX << " " << sensorsData.rawCompassY << " " <<
// sensorsData.rawCompassZ << std::endl;
sendData(outArray);
}
loopCount++;
if (loopCount == calibrationDelay)
{
calibration = false;
gyroState.offsetX /= calibrationDelay;
gyroState.offsetY /= calibrationDelay;
gyroState.offsetZ /= calibrationDelay;
ahrsProcessor.setGyroOffsets(gyroState.offsetX, gyroState.offsetY,
gyroState.offsetZ);
ahrsProcessor.setOffsetQuaternion(offsetQ);
showCalibration(false);
}
}
#if WRITE_SENSORS_DATA
stream.close();
#endif
}
void setup(){
// A visual Queue that the System is in
// Setup Mode.
// Pin 14 -> LED ON
digitalWrite(SetupLED, HIGH);
// ----- PRELIMINARY PROCEDURES ------
// -> init serial Connection to LCD
initLCD();
LCDprintln("***** Starting Up *****");
delay(100);
LCDprintln(" [OK] ");
// ----- SPLASH SCREEN PROCEDURES ------
// -> splash Screen
// -> ask for connections
// -> Store USB?
#ifdef MEGA
toggleSplashScreen();
delay(1000);
askForConnectionTypes();
askToStoreData();
#endif
// ----- DEBUG STARTUP PROCEDURES ------
#ifdef DEBUG
toggleSplashScreen();
delay(1000);
LCDprintln("***** DEBUG MODE *****");
startDebugSequence();
#endif
// ----- PINS // INT // ISR PROCEDURES ------
// -> init buttons
// -> init LED pins
// -> init Digital pins
// -> init Sensors
// -> attach interrupts
#ifdef MEGA
initbuttons();
#else
LCDprintln("In DEBUG mode no input required");
#endif
#ifdef MEGA
initLEDPins();
initDigitalPins();
initSensors();
#else
LCDprintln("No need for inputs random numbers\n
sent to the PC/XBee.");
#endif
attachInterrupts();
// ----- COMMS STARTUP PROCEDURES ------
// -> init Keyboard
// -> init serial Connection to XBee
// -> init serial Connection to PC
#ifdef XBeeConnected
initXbee();
LCDdisplayConnectedIcon(Connection);
#endif
#ifdef USBConnected
initUSB();
LCDdisplayConnectedIcon(Connection);
#endif
#ifdef StoreUSB
initVDIP();
LCDdisplayConnectedIcon(Connection);
printHeaders();
#endif
// ----- MEM. & MENUS STARTUP PROCEDURES ------
// -> init EEPROM
// -> +1 to session ID
// -> Verify Version ID
// -> init Menu System
// -> Startup
initEEPROM();
initMenus();
// A visual Queue that the System is now past
// the Setup Mode.
// Pin 14 -> LED OFF
digitalWrite(SetupLED, LOW);
}
SensorBoard::SensorBoard()
{
initSensors();
}
int main(int argc, char* argv[]) {
int i = 0; // indice usato per i cicli for
int n = 0; // variabile per il numero di sensori
xQueueHandle queue[NUM_PRIORITIES];
Sync_Init sync_sensor;
initNode();
initSensors();
// initNetwork(); funzione fornita dal livello rete
if ( join(getID()) == 0) { // supponendo sia questa la firma
// scrive nel log che si e' verificato un errore di autenticazione
while(1);
}
for (i = 0; i < NUM_PRIORITIES; i++)
queue[i] = xQueueCreate(MAX_QUEUE_LENGTH, sizeof(Message));
if (xTaskCreate(asyncRequestManage,// nome della funzione
"Gestione richieste asincrone",
configMINIMAL_STACK_SIZE,
queue(1), // parametri della funzione: queue(1)
3) != pdTRUE) {
// scrive nel log che si `e verificato un errore di gestione della memoria
while(1);
}
n = getNumSensors();
for (i = 0; i < n; i++) {
if (getPeriod(getSensorID(i)) > 0) {
sync_sensor.ID = getSensorID(i);
sync_sensor.period = getPeriod(getSensorID(i));
if (xTaskCreate(syncSensorManage,// nome della funzione
"Task di gestione di un sensore sincrono ",
configMINIMAL_STACK_SIZE,
(void *)&sync_sensor, // parametri della funzione
2) != pdTRUE) {
// scrive nel log che si `e verificato un errore di gestione della memoria
while(1);
}
}
}
if (xTaskCreate(sendOverNetwork,// nome della funzione
"Task per l'invio dei messaggi",
configMINIMAL_STACK_SIZE,
(void *)queue, // parametri della funzione
1) != pdTRUE) {
// scrive nel log che si `e verificato un errore di gestione della memoria
while(1);
}
vTaskStartScheduler();
while (1);
}
int main(int argc, char *argv[])
{
struct sensorGrid readings;
int fd, wd, idx;
char fullname[FILENAME_MAX], tmpname[FILENAME_MAX], logCommands[4096];
char hostname[200];
int er=gethostname(hostname,sizeof(hostname));
printf("Hostname=%s\n",hostname);
readConfiguration(CONFIGFILENAME);
if (argc>=2)
{
if (strcasecmp(argv[1],"retry")==0)
{ char dateName[32], currDate[32];
if(argc==3) strcpy(dateName,argv[2]);
else {
time_t now = (time_t) time(0);
struct tm *gmtm = (struct tm *) gmtime(&now);
strftime(dateName, sizeof(dateName), "retry_%Y-%m-%d.txt", gmtm);
}
retryToTransmit(dateName);
return;
}
}
//if(argc != 3 && argc != 4) {
//fprintf(stderr, "Usage: %s foldername filename [output]\n", argv[0]);
//return 1;
//}
//if(argc == 4) {
if(Configuration.commandSerial[0]) {
otp = openSerialPort(Configuration.commandSerial, B9600);
if(otp) {
itp = otp;
} else {
perror( "Unable to append to output device" );
otp = stdout;
itp = stdin;
}
} else {
otp = stdout;
itp = stdin;
}
/* read configuration */
for(idx = 0; idx < MAX_PROP; idx++) status[idx] = NULL;
readStatus(status);
/* set up communication with the siren */
if (wiringPiSetupGpio () == -1)
{
perror( "Can't initialise wiringPi" );
return -2;
}
initSiren(SIREN_PIN);
initBigDisplay();
readings.pressure = 0;
readings.temperature = 0;
readings.distance = 0;
initSensors();
initServo(SERVO_PIN, itp);
startServo();
sleep(1); // sleep 1 s to have at least 1 sensor value
initAnalysis();
// lcd = lcdInit (2, 16, 4, RS, STRB, 0, 1, 2, 3, 4, 5, 6, 7);
startSensorsReading(&readings);
if(Configuration.watchFolder[0] && Configuration.watchFile) {
sprintf(tmpname, "%s/%s", argv[1], argv[2]);
realpath(tmpname, fullname);
}
/* set up file watcher */
fd = inotify_init ();
if (fd < 0) {
perror ("unable to initialize inotify");
return -1;
}
if( Configuration.watchFolder[0] )
wd = inotify_add_watch (fd, Configuration.watchFolder, IN_CLOSE_WRITE );
else
wd = 0;
if (wd < 0) {
perror ("unable to add inotify watch");
return -2;
}
/* loop on file change events and execute commands */
while( 1 ) {
char buf[BUF_LEN];
int len, i = 0, process = 0;
len = read (fd, buf, BUF_LEN);
if (len < 0) {
if (errno == EINTR)
; /* need to reissue system call */
else
perror ("error reading inotify stream");
} else if (!len)
; /* BUF_LEN too small? */
while (i < len) {
struct inotify_event *event;
event = (struct inotify_event *) &buf[i];
// printf ("wd=%d mask=%u cookie=%u len=%u\n",
// event->wd, event->mask,
// event->cookie, event->len);
if (event->len && strcmp(argv[2], event->name) == 0)
process++;
i += EVENT_SIZE + event->len;
}
if(process) { // It doesn't matter how many times the file changed, but it is good for debugging
char *line = NULL, *ptr, *end, **commandName, *displayLines[MAX_LINES];
FILE *fp = 0;
int i, len, chars, cmd, statusDirty = 0, cmdPosition = 0;
process = 0;
fp=fopen(fullname, "r");
if(fp) {
unlink(fullname); // the name is removed from the filesystem, the file isn't until it is closed
// if a new file is created, it accumulates event in the watcher, and we'll cycle again next round
while ((chars = getline(&line, &len, fp)) != -1) {
ptr = line;
while(*ptr != ':') {
*ptr=toupper(*ptr);
ptr++;
}
for( cmd=0, ptr = *(commandName = commands); ptr; ptr=*(++commandName), cmd++)
if( !strncmp(ptr, line, strlen(ptr)) )
{ // the command is present in the list and can be sent to the panel: it double checks the data from php
switch(cmd)
{
case 7: // SIREN
ptr = line + 1 + strlen(ptr); // skip property name and colon
end = ptr + strlen(ptr);
while( *--end == 13 || *end == 10) *end = '\0';
siren(ptr);
if(cmdPosition + strlen(line) < 3072)
cmdPosition += sprintf(logCommands + cmdPosition, "%s;", line );
statusDirty = -1;
break;
case 8: // SPEAKER
break;
case 9: // DELETE
break;
case 10: // RESET
break;
case 11: // PWR
break;
case 12: // TIMEZONE
break;
case 13: // NAME
break;
case 14: // RESTORE
break;
case 15: // TIMETO
break;
case 16: // IPADDR
break;
case 17: // SUBNET
break;
case 18: // GSM
break;
case 19: // GSMRES
break;
default: // set property
if(status[cmd]) free(status[cmd]);
ptr = line + 1 + strlen(ptr); // skip property name and colon
end = ptr + strlen(ptr); // end of command to retrieve EOL
while( *--end == 13 || *end == 10) *end = '\0';
status[cmd] = strdup(ptr);
statusDirty = -1;
fprintf(otp, "%s\r\n", line);
fflush(otp);
if(cmdPosition + strlen(line) < 3072)
cmdPosition += sprintf(logCommands + cmdPosition, "%s;", line );
break;
}
break;
}
}
if(line) free(line);
fclose(fp);
} else {
perror("Unable to open the update file");
}
if(statusDirty)
writeStatus();
logCommandAsync(logCommands);
for( i = 0; i < MAX_LINES; i++)
displayLines[i] = status[LINE1 + i];
len = 4;
while(i && !displayLines[--i]) len--;
bigDisplay(len, displayLines);
}
}
return 0;
}
/*!
* @brief Initializes the whole system and runs the desired application
*
* This is the main function of the project. It calls initialization functions
* of the MCU and the sensors. In the infinite loop it repeatedly checks
* the USART module read buffer and Streams sensor data periodically (100 ms) via USART.
*
*/
int main(void)
{
/********************* Initialize global variables **********************/
bmf055_input_state = USART_INPUT_STATE_PRINT_DATA;
/************************* Initializations ******************************/
/*Initialize SAMD20 MCU*/
system_init();
/*Initialize clock module of SAMD20 MCU - Internal RC clock*/
//clock_initialize(); // done via conf_clocks.h --> ASF
/*SPI master for communicating with sensors*/
spi_initialize();
/*eeprom emulator for configuration storage */
eeprom_emulator_initialize();
/*Initialize timers */
tc_initialize();
/*Initialize UART for communication with PC*/
usart_initialize();
/*Enable the system interrupts*/
system_interrupt_enable_global();/* All interrupts have a priority of level 0 which is the highest. */
/* Initialize the sensors */
bmf055_sensors_initialize();
readEEPROM();
checkFirstTime(0);
//readEEPROM();
configureReceiver();
initSensors();
previousTime = micros();
calibratingG = 400;
f.SMALL_ANGLES_25=1; // important for gyro only conf
if(conf.copterType == 0){//0=Bi,1=Tri,2=QUADP,3=QUADX,4=Y4,5=Y6,6=H6P,7=H6X,8=Vtail4
MULTITYPE = 4;
NUMBER_MOTOR = 2;
}
if(conf.copterType == 1){
MULTITYPE = 1;
NUMBER_MOTOR = 3;
}
if(conf.copterType == 2){
MULTITYPE = 2;
NUMBER_MOTOR = 4;
}
if(conf.copterType == 3){
MULTITYPE = 3;
NUMBER_MOTOR = 4;
}
if(conf.copterType == 4){
MULTITYPE = 9;
NUMBER_MOTOR = 4;
}
if(conf.copterType == 5){
MULTITYPE = 6;
NUMBER_MOTOR = 6;
}
if(conf.copterType == 6){
MULTITYPE = 7;
NUMBER_MOTOR = 6;
}
if(conf.copterType == 7){
MULTITYPE = 10;
NUMBER_MOTOR = 6;
}
if(conf.copterType == 8){
MULTITYPE = 17;
NUMBER_MOTOR = 4;
}
initOutput();
/************************** Infinite Loop *******************************/
while (true)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint8_t beepon = 0;
uint8_t axis,i;
int16_t error,errorAngle;
int16_t delta,deltaSum;
int16_t PTerm=0,ITerm=0,PTermACC=0,ITermACC=0,PTermGYRO=0,ITermGYRO=0,DTerm=0;
static int16_t lastGyro[3] = {0,0,0};
static int16_t delta1[3],delta2[3];
static int16_t errorGyroI[3] = {0,0,0};
static int16_t errorAngleI[2] = {0,0};
static uint32_t rcTime = 0;
static uint32_t BeepTime = 0;
static uint8_t stickarmed = 0;
//static int16_t initialThrottleHold;
if(!rcOptions[BOXARM] && stickarmed == 0 && f.ARMED == 0){
if(rcData[YAW]<conf.MINCHECK && rcData[ROLL]>conf.MAXCHECK){
conf.calibState=1;
writeParams(1);
while(true){
//blinkLED(10,30,1);
}
}
}
while(SetupMode == 1){
checkSetup();
}
if(conf.RxType == 1 || conf.RxType == 2){
if (rcFrameComplete) computeRC();
}
if(!rcOptions[BOXARM] && stickarmed == 0) {
f.ARMED = 0;
}
if (currentTime > rcTime ) { // 50Hz
rcTime = currentTime + 20000;
if(failsave < 250)failsave++;
debug[0] = failsave;
if(conf.RxType != 1 && conf.RxType != 2){
computeRC();
}
if ((rcData[THROTTLE] < conf.MINCHECK && s3D == 0) || (rcData[THROTTLE] > (1500-conf.MIDDLEDEADBAND) && rcData[THROTTLE] < (1500+conf.MIDDLEDEADBAND) && s3D == 1 && f.ARMED == 0)) {
errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0;
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
rcDelayCommand++;
if (rcData[YAW] < conf.MINCHECK && rcData[PITCH] < conf.MINCHECK && !f.ARMED) {
if (rcDelayCommand == 20 && failsave < 20) {
calibratingG=400;
}
}else if (rcData[YAW] > conf.MAXCHECK && rcData[PITCH] > conf.MAXCHECK && !f.ARMED) {
if (rcDelayCommand == 20) {
previousTime = micros();
}
}else if (conf.activate[BOXARM] > 0) {
if ( rcOptions[BOXARM] && f.OK_TO_ARM && good_calib) {
f.ARMED = 1;
stickarmed = 0;
} else if (f.ARMED) f.ARMED = 0;
rcDelayCommand = 0;
} else if ( (rcData[YAW] < conf.MINCHECK ) && f.ARMED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 0) {
if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ( (rcData[YAW] > conf.MAXCHECK ) && rcData[PITCH] < conf.MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 0) {
if (rcDelayCommand == 20 && good_calib) {
f.ARMED = 1;
stickarmed = 1;
}
} else if ( (rcData[ROLL] < conf.MINCHECK ) && f.ARMED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 1) {
if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ( (rcData[ROLL] > conf.MAXCHECK ) && rcData[PITCH] < conf.MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 1) {
if (rcDelayCommand == 20 && good_calib) {
f.ARMED = 1;
stickarmed = 1;
}
} else
rcDelayCommand = 0;
} else if (rcData[THROTTLE] > conf.MAXCHECK && !f.ARMED) {
if (rcData[YAW] < conf.MINCHECK && rcData[PITCH] < conf.MINCHECK) { // throttle=max, yaw=left, pitch=min
if (rcDelayCommand == 20) calibratingA=400;
rcDelayCommand++;
} else if (rcData[PITCH] > conf.MAXCHECK) {
conf.angleTrim[PITCH]+=2;writeParams(1);
} else if (rcData[PITCH] < conf.MINCHECK) {
conf.angleTrim[PITCH]-=2;writeParams(1);
} else if (rcData[ROLL] > conf.MAXCHECK) {
conf.angleTrim[ROLL]+=2;writeParams(1);
} else if (rcData[ROLL] < conf.MINCHECK) {
conf.angleTrim[ROLL]-=2;writeParams(1);
} else {
rcDelayCommand = 0;
}
}
uint16_t auxState = 0;
for(i=0;i<4;i++)
auxState |= (rcData[AUX1+i]<1300)<<(3*i) | (1300<rcData[AUX1+i] && rcData[AUX1+i]<1700)<<(3*i+1) | (rcData[AUX1+i]>1700)<<(3*i+2);
for(i=0;i<CHECKBOXITEMS;i++)
rcOptions[i] = (auxState & conf.activate[i])>0;
if(failsave > 200 && f.ARMED){
rcOptions[BOXACC] = 1;
s3D = 0;
rcData[THROTTLE] = 1190;
rcCommand[THROTTLE] = 1190;
}
if (rcOptions[BOXACC] && s3D == 0) {
// bumpless transfer to Level mode
if (!f.ACC_MODE) {
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
f.ACC_MODE = 1;
}
} else {
// failsafe support
f.ACC_MODE = 0;
}
if (rcOptions[BOXBEEP]) {
f.FSBEEP = 1;
if(currentTime > BeepTime){
BeepTime = currentTime + 50000;
if(beepon == 0){
if(conf.RxType == 0){
//digitalWrite(A2,HIGH);
}else{
//digitalWrite(8,HIGH);
}
beepon = 1;
}else{
if(conf.RxType == 0){
//digitalWrite(A2,LOW);
}else{
//digitalWrite(8,LOW);
}
beepon = 0;
}
}
} else {
f.FSBEEP = 0;
if(conf.RxType == 0){
//digitalWrite(A2,LOW);
}else{
//digitalWrite(8,LOW);
}
}
if (rcOptions[BOXHORIZON] && s3D == 0) {
// bumpless transfer to Horizon mode
if (!f.HORIZON_MODE) {
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
f.HORIZON_MODE = 1;
}
} else {
f.HORIZON_MODE = 0;
}
if (rcOptions[BOX3D] && conf.F3D == 1) {
if(f.ARMED == 0 && s3D == 0){
s3D = 1;
f.ACC_MODE = 0;
f.HORIZON_MODE = 0;
}
} else if(f.ARMED == 0){
s3D = 0;
}
if (rcOptions[BOXARM] == 0) f.OK_TO_ARM = 1;
}
computeIMU();
int16_t prop;
if (f.HORIZON_MODE)
prop = max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])); // range [0;500]
if (f.ACC_MODE){
if(Zadd > 0)Zadd--;
if(Zadd < 0)Zadd++;
}else{
Zadd = 0;
}
//**** PITCH & ROLL & YAW PID ****
for(axis=0;axis<3;axis++) {
if ((f.ACC_MODE || f.HORIZON_MODE) && axis<2 ) { //LEVEL MODE
// 50 degrees max inclination
errorAngle = constrain(2*rcCommand[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //16 bits is ok here
#ifdef LEVEL_PDF
PTermACC = -(int32_t)angle[axis]*conf.P8[PIDLEVEL]/100 ;
#else
PTermACC = (int32_t)errorAngle*conf.P8[PIDLEVEL]/100 ; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
#endif
PTermACC = constrain(PTermACC,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5);
errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // WindUp //16 bits is ok here
ITermACC = ((int32_t)errorAngleI[axis]*conf.I8[PIDLEVEL])>>12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
}
if ( !f.ACC_MODE || f.HORIZON_MODE || axis == 2 ) { // MODE relying on GYRO or YAW axis
if (abs(rcCommand[axis])<350) error = rcCommand[axis]*10*8/conf.P8[axis] ; // 16 bits is needed for calculation: 350*10*8 = 28000 16 bits is ok for result if P8>2 (P>0.2)
else error = (int32_t)rcCommand[axis]*10*8/conf.P8[axis] ; // 32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
error -= gyroData[axis];
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis]+error,-16000,+16000); // WindUp 16 bits is ok here
if (abs(gyroData[axis])>640) errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis]/125*conf.I8[axis])>>6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
}
if ( f.HORIZON_MODE && axis<2) {
PTerm = ((int32_t)PTermACC*(500-prop) + (int32_t)PTermGYRO*prop)/500;
ITerm = ((int32_t)ITermACC*(500-prop) + (int32_t)ITermGYRO*prop)/500;
} else {
if ( f.ACC_MODE && axis<2) {
PTerm = PTermACC;
ITerm = ITermACC;
} else {
PTerm = PTermGYRO;
ITerm = ITermGYRO;
}
}
if (abs(gyroData[axis])<160) PTerm -= gyroData[axis]*dynP8[axis]/10/8; // 16 bits is needed for calculation 160*200 = 32000 16 bits is ok for result
else PTerm -= (int32_t)gyroData[axis]*dynP8[axis]/10/8; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis]+delta2[axis]+delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
if (abs(deltaSum)<640) DTerm = (deltaSum*dynD8[axis])>>5; // 16 bits is needed for calculation 640*50 = 32000 16 bits is ok for result
else DTerm = ((int32_t)deltaSum*dynD8[axis])>>5; // 32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm - DTerm;
}
void setup() {
#if !defined(GPS_PROMINI)
UARTInit(115200);
#endif
LEDPIN_PINMODE;
SHIELDLED_PINMODE;
//POWERPIN_PINMODE;
//BUZZERPIN_PINMODE;
//STABLEPIN_PINMODE;
//POWERPIN_OFF;
/* Initialize GPIO (sets up clock) */
GPIOInit();
init_microsec();
enable_microsec();
init_timer16PWM();
enable_PWMtimer();
/******** special version of MultiWii to calibrate all attached ESCs ************/
#if defined(ESC_CALIB_CANNOT_FLY)
writeAllMotors(ESC_CALIB_HIGH);
delayMs(3000);
writeAllMotors(ESC_CALIB_LOW);
delayMs(500);
while (1) {
delayMs(5000);
blinkLED(2,20, 2);
}
// statement never reached
#endif
writeAllMotors(MINCOMMAND);
delayMs(300);
readEEPROM();
checkFirstTime();
configureReceiver();
#if defined(OPENLRSv2MULTI)
initOpenLRS();
#endif
initSensors();
#if defined(I2C_GPS) || defined(GPS_SERIAL) || defined(GPS_FROM_OSD)||defined(I2C_NAV)
GPS_set_pids();
#endif
previousTime = micros();
#if defined(GIMBAL)
calibratingA = 400;
#endif
calibratingG = 400;
calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
#if defined(POWERMETER)
for(uint8_t i=0;i<=PMOTOR_SUM;i++)
pMeter[i]=0;
#endif
#if defined(ARMEDTIMEWARNING)
ArmedTimeWarningMicroSeconds = (ARMEDTIMEWARNING *1000000);
#endif
/************************************/
#if defined(GPS_SERIAL)
SerialOpen(GPS_SERIAL,GPS_BAUD);
delay(400);
for(uint8_t i=0;i<=5;i++){
GPS_NewData();
LEDPIN_ON
delay(20);
LEDPIN_OFF
delay(80);
}
if(!GPS_Present){
SerialEnd(GPS_SERIAL);
SerialOpen(0,SERIAL_COM_SPEED);
}
#if !defined(GPS_PROMINI)
GPS_Present = 1;
#endif
GPS_Enable = GPS_Present;
#endif
/************************************/
#if defined(I2C_GPS) || defined(TINY_GPS) || defined(GPS_FROM_OSD)|| defined(I2C_NAV)
GPS_Enable = 1;
#endif
#if defined(LCD_ETPP) || defined(LCD_LCD03) || defined(OLED_I2C_128x64)
initLCD();
#endif
#ifdef LCD_TELEMETRY_DEBUG
telemetry_auto = 1;
#endif
#ifdef LCD_CONF_DEBUG
configurationLoop();
#endif
#ifdef LANDING_LIGHTS_DDR
init_landing_lights();
#endif
#if defined(LED_FLASHER)
init_led_flasher();
led_flasher_set_sequence(LED_FLASHER_SEQUENCE);
#endif
f.SMALL_ANGLES_25=1; // important for gyro only conf
//initialise median filter structures
#ifdef MEDFILTER
#ifdef SONAR
initMedianFilter(&SonarFilter, 5);
#endif
#endif
initWatchDog();
}
