/* -------------------------------------------------------------------- */ void stmSendTrajectory(float elevatorTrajectory[5], float aileronTrajectory[5]) { char crc = 0; sendChar(usart_buffer_stm, 'a', &crc); // this character initiates the transmission sendChar(usart_buffer_stm, 1 + 5*4 + 5*4, &crc); // this will be the size of the message sendChar(usart_buffer_stm, 't', &crc); // id of the message int i; // send the elevator trajectory for (i = 0; i < 5; i++) { sendFloat(usart_buffer_stm, elevatorTrajectory[i], &crc); } // send the aileron trajectory for (i = 0; i < 5; i++) { sendFloat(usart_buffer_stm, aileronTrajectory[i], &crc); } // at last send the crc, ends the transmission sendChar(usart_buffer_stm, crc, &crc); }
/* -------------------------------------------------------------------- */ void stmSetKalmanPosition(float elevatorPos, float aileronPos) { char crc = 0; sendChar(usart_buffer_stm, 'a', &crc); // this character initiates the transmission sendChar(usart_buffer_stm, 1 + 2*4, &crc); // this will be the size of the message sendChar(usart_buffer_stm, '3', &crc); // id of the message sendFloat(usart_buffer_stm, elevatorPos, &crc); sendFloat(usart_buffer_stm, aileronPos, &crc); // at last send the crc, ends the transmission sendChar(usart_buffer_stm, crc, &crc); }
/* -------------------------------------------------------------------- */ void stmResetKalman(float initElevator, float initAileron) { char crc = 0; sendChar(usart_buffer_stm, 'a', &crc); // this character initiates the transmission sendChar(usart_buffer_stm, 1 + 2*4, &crc); // this will be the size of the message sendChar(usart_buffer_stm, '2', &crc); // id of the message sendFloat(usart_buffer_stm, initElevator, &crc); sendFloat(usart_buffer_stm, initAileron, &crc); // at last send the crc, ends the transmission sendChar(usart_buffer_stm, crc, &crc); }
/* -------------------------------------------------------------------- */ void stmSendSetpoint(float elevatorSetpoint, float aileronSetpoint) { char crc = 0; sendChar(usart_buffer_stm, 'a', &crc); // this character initiates the transmission sendChar(usart_buffer_stm, 1 + 2*4, &crc); // this will be the size of the message sendChar(usart_buffer_stm, 's', &crc); // id of the message // sends the payload sendFloat(usart_buffer_stm, elevatorSetpoint, &crc); sendFloat(usart_buffer_stm, aileronSetpoint, &crc); // at last send the crc, ends the transmission sendChar(usart_buffer_stm, crc, &crc); }
/* -------------------------------------------------------------------- */ void stmSendMeasurement(float elevSpeed, float aileSpeed, int16_t elevInput, int16_t aileInput) { char crc = 0; sendChar(usart_buffer_stm, 'a', &crc); // this character initiates the transmission sendChar(usart_buffer_stm, 1+12, &crc); // this will be the size of the message sendChar(usart_buffer_stm, '1', &crc); // id of the message // sends the payload sendFloat(usart_buffer_stm, elevSpeed, &crc); sendFloat(usart_buffer_stm, aileSpeed, &crc); sendInt16(usart_buffer_stm, elevInput, &crc); sendInt16(usart_buffer_stm, aileInput, &crc); // at last send the crc, ends the transmission sendChar(usart_buffer_stm, crc, &crc); }
PdBase& PdBase::operator<<(const Float& var) { if(PdContext::instance().bMsgInProgress) { cerr << "Pd: Can not send Float, message in progress" << endl; return *this; } sendFloat(var.dest.c_str(), var.value); return *this; }
// Data is sent via wireless serial link to ground station // data packet format: *p%ROLL%PITCH%ALTITUDE%AIRSPEED%LATTITUDE%LONGITUDE%HEADING%ms%secondee // Not anymore: now is bytewise transmission of floats // Total number of bytes: 11 (*p% type) + ~ 50 (data) = 61 bytes *4x/second = 244 bytes/s. Each transmission is under outgoing buffer (128 bytes) and baud // No other serial communication can be done in other classes!!! void Communicator::sendData() { /* XBEE_SERIAL.print("*p%"); XBEE_SERIAL.print(roll); //6? XBEE_SERIAL.print('%'); XBEE_SERIAL.print(pitch); //6? XBEE_SERIAL.print('%'); XBEE_SERIAL.print(altitude); //6 XBEE_SERIAL.print('%'); XBEE_SERIAL.print(GPS.speed); //5 XBEE_SERIAL.print('%'); XBEE_SERIAL.print(GPS.latitude, 4); //8 //second argument is number of decimal places XBEE_SERIAL.print('%'); XBEE_SERIAL.print(GPS.longitude, 4); //8 XBEE_SERIAL.print('%'); XBEE_SERIAL.print(GPS.angle, 2); //6 XBEE_SERIAL.print('%'); XBEE_SERIAL.print(GPS.milliseconds); //2 XBEE_SERIAL.print('%'); XBEE_SERIAL.print(GPS.seconds); //3 XBEE_SERIAL.print("ee"); */ // Change to: (total of 27 (35) bytes) XBEE_SERIAL.print("*p"); //sendFloat((float)roll); //No longer send //sendFloat((float)pitch); //No longer send sendFloat((float)altitude); sendFloat(GPS.speed); sendFloat(GPS.latitude); sendFloat(GPS.longitude); sendFloat(GPS.angle); sendUint16_t(GPS.milliseconds); sendUint8_t(GPS.seconds); XBEE_SERIAL.print("ee"); }
void Com::sendSettings() { sendPacketStart(PACKET_SETTINGS); sendFloat(g_rollPid.P); sendFloat(g_rollPid.I); sendFloat(g_rollPid.D); sendFloat(g_pitchPid.P); sendFloat(g_pitchPid.I); sendFloat(g_pitchPid.D); sendFloat(g_yawPid.P); sendFloat(g_yawPid.I); sendFloat(g_yawPid.D); sendFloat(g_userControlGain); sendFloat(g_pidWindupGuard); sendOk(); }
int main(int argc, char *argv[]) { int pid = -1; //PID float temp = 0; //inital temperature from command line float downTemp = 0; float parentTemp = 0; float upTemp = 0; //parses command line arguments and updates pid and initial temperature/exits on fail if(!getArgumentValues(parseArguments(argc, argv), pid, temp)) exit(0); mqd_t mqueue; /*message queue*/ mqd_t parent; mqd_t child_1; mqd_t child_2; struct mq_attr ma; ma.mq_flags = 0; ma.mq_maxmsg = 10; ma.mq_msgsize = 33; ma.mq_curmsgs = 0; float child1Value; float child2Value; //sendFloat(mqueue, temp); //qout << receiveFloat(mqueue); //mq_close(mqueue); //mq_unlink(P0); switch(pid){ case 0: { mqueue = openMailbox(P0, ma); child_1 = openMailbox(P1, ma); child_2 = openMailbox(P2, ma); sendFloat(child_1, temp, 0.0); //false flags as sent down sendFloat(child_2, temp, 0.0); break; } case 1: parent = openMailbox(P0, ma); mqueue = openMailbox(P1, ma); child_1 = openMailbox(P3, ma); child_2 = openMailbox(P4, ma); break; case 2: parent = openMailbox(P0, ma); mqueue = openMailbox(P2, ma); child_1 = openMailbox(P5, ma); child_2 = openMailbox(P6, ma); break; case 3: parent = openMailbox(P1, ma); mqueue = openMailbox(P3, ma); break; case 4: parent = openMailbox(P1, ma); mqueue = openMailbox(P4, ma); break; case 5: parent = openMailbox(P2, ma); mqueue = openMailbox(P5, ma); break; case 6: parent = openMailbox(P2, ma); mqueue = openMailbox(P6, ma); break; } bool isStable = false; while(!isStable) { if(pid == 0) { if(numMessages(mqueue) > 1) { float cPid = 10; float c2Pid = 10; float child1Value = receiveFloat(mqueue, cPid); float child2Value = receiveFloat(mqueue, c2Pid); upTemp = (temp + child1Value + child2Value)/3.0; qout << "Process " << pid << " current temperature " << upTemp << endl; if(qAbs(upTemp - temp) <= .01) { temp = upTemp; isStable = true; sendFloat(child_1, temp, 20); sendFloat(child_2, temp, 20); } else { temp = upTemp; sendFloat(child_1, temp, pid); sendFloat(child_2, temp, pid); } } } if(pid == 1 || pid == 2) { if(hasMessages(mqueue)) { float rPid = 10; float value = receiveFloat(mqueue, rPid); if(rPid == 20) { isStable = true; sendFloat(child_1, temp, 20); sendFloat(child_1, temp, 20); } else if(rPid == 0) { float downTemp = (temp + value)/2.0; temp = downTemp; sendFloat(child_1, downTemp, pid); sendFloat(child_2, downTemp, pid); } else if(rPid == 3 || rPid == 4 || rPid == 5 || rPid == 6) { float child1_value = value; float sPid = 10; float child2_value = receiveFloat(mqueue, sPid); if(sPid == 3 || sPid == 4 || sPid == 5 || sPid == 6) { upTemp = (temp + child1_value + child2_value)/3.0; temp = upTemp; qout << "Process " << pid << " current temperature " << upTemp << endl; sendFloat(parent, upTemp, pid); } } } } if(pid == 3 || pid == 4 || pid == 5 || pid ==6) { if(hasMessages(mqueue)) { float rPid; parentTemp = receiveFloat(mqueue, rPid); if(rPid == 20) { isStable = true; } else { downTemp = (temp + parentTemp)/2.0; temp = downTemp; sendFloat(parent, downTemp, pid); //true flags it as sent up qout << "Process " << pid << " current temperature " << downTemp << endl; } } } } qout << "Process " << pid << " final temperature " << temp << endl; mq_close(parent); mq_close(child_1); mq_close(child_2); mq_close(mqueue); mq_unlink(P0); mq_unlink(P1); mq_unlink(P2); mq_unlink(P3); mq_unlink(P4); mq_unlink(P5); mq_unlink(P6); return 0; } /*end main()*/
void Him::watch() { watcher_t w = create_watcher(); shm_watch(him_settings, w); shm_watch(him, w); // Initial write: // TODO: resync before this? sendFloat(16, him_settings.heading_offset); sendFloat(18, him_settings.pitchoffset); sendFloat(20, him_settings.rolloffset); sendFloat(22, him_settings.xacceloffset); sendFloat(24, him_settings.yacceloffset); sendFloat(26, him_settings.zacceloffset); sendInt16(28, him_settings.xcompoffset); sendInt16(29, him_settings.ycompoffset); sendInt16(30, him_settings.zcompoffset); while (isActive()) { wait_watcher(w, false); // Don't block if there has been an update // we want to make sure it gets through shm_getg(him_settings, him_settings_new); shm_getg(him, him_new); if (him_settings_new.heading_offset != him_settings.heading_offset) { sendFloat(16, (him_settings_new.heading_offset)); him_settings.heading_offset = him_settings_new.heading_offset; } if (him_settings_new.pitchoffset != him_settings.pitchoffset) { sendFloat(18, (him_settings_new.pitchoffset)); him_settings.pitchoffset = him_settings_new.pitchoffset; } if (him_settings_new.rolloffset != him_settings.rolloffset) { sendFloat(20, (him_settings_new.rolloffset)); him_settings.rolloffset = him_settings_new.rolloffset; } if (him_settings_new.xacceloffset != him_settings.xacceloffset) { sendFloat(22, (him_settings_new.xacceloffset)); him_settings.xacceloffset = him_settings_new.xacceloffset; } if (him_settings_new.yacceloffset != him_settings.yacceloffset) { sendFloat(24, (him_settings_new.yacceloffset)); him_settings.yacceloffset = him_settings_new.yacceloffset; } if (him_settings_new.zacceloffset != him_settings.zacceloffset) { sendFloat(26, (him_settings_new.zacceloffset)); him_settings.zacceloffset = him_settings_new.zacceloffset; } if (him_settings_new.xcompoffset != him_settings.xcompoffset) { sendInt16(28, (him_settings_new.xcompoffset)); him_settings.xcompoffset = him_settings_new.xcompoffset; } if (him_settings_new.ycompoffset != him_settings.ycompoffset) { sendInt16(29, (him_settings_new.ycompoffset)); him_settings.ycompoffset = him_settings_new.ycompoffset; } if (him_settings_new.zcompoffset != him_settings.zcompoffset) { sendInt16(30, (him_settings_new.zcompoffset)); him_settings.zcompoffset = him_settings_new.zcompoffset; } } destroy_watcher(w); }