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