void Junxion::sendData() {
sendHeader(DATA_RESPONSE, _dataSize);
// Send digital input states
uint16_t state = 0;
uint8_t pos = 0;
for (uint8_t i = 0; i < _digitalInputCount; ++i) {
if (_digitalInputs[i].active) {
state = state | (1 << pos);
}
++pos;
if (pos >= 16) {
sendInt16(state);
state = 0;
pos = 0;
}
}
if (pos > 0) {
sendInt16(state);
}
// Send analog pin states
for (uint8_t i = 0; i < _analogInputCount; ++i) {
sendInt16(_analogInputs[i].value);
}
}
void Pod::watch() {
watcher_t w = create_watcher();
shm_watch(switches, w);
shm_watch(mission_start_switch, w);
shm_watch(merge_status, w);
// Initial write:
// TODO: resync before this?
sendInt16(30, mission_start_switch.mission_light);
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(switches, switches_new);
shm_getg(mission_start_switch, mission_start_switch_new);
shm_getg(merge_status, merge_status_new);
if (mission_start_switch_new.mission_light != mission_start_switch.mission_light) {
sendInt16(30, (mission_start_switch_new.mission_light));
mission_start_switch.mission_light = mission_start_switch_new.mission_light;
}
}
destroy_watcher(w);
}
/* -------------------------------------------------------------------- */
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);
}
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);
}
void Junxion::sendJunxionId() {
sendHeader(JUNXION_ID_RESPONSE, 2);
sendInt16(JUNXION_ID);
}
void Actuators::watch() {
watcher_t w = create_watcher();
shm_watch(switches, w);
shm_watch(actuator_desires, w);
shm_watch(actuator_status, w);
// Initial write:
// TODO: resync before this?
sendInt16(16, actuator_desires.trigger_01);
sendInt16(17, actuator_desires.trigger_02);
sendInt16(18, actuator_desires.trigger_03);
sendInt16(19, actuator_desires.trigger_04);
sendInt16(20, actuator_desires.trigger_05);
sendInt16(21, actuator_desires.trigger_06);
sendInt16(22, actuator_desires.trigger_07);
sendInt16(23, actuator_desires.trigger_08);
sendInt16(24, actuator_desires.trigger_09);
sendInt16(25, actuator_desires.trigger_10);
sendInt16(26, actuator_desires.trigger_11);
sendInt16(27, actuator_desires.trigger_12);
sendInt16(28, actuator_desires.trigger_13);
sendInt16(29, actuator_desires.trigger_14);
sendInt16(30, actuator_desires.motor_pwm_1);
sendInt16(31, actuator_desires.motor_pwm_2);
sendInt16(32, switches.soft_kill);
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(switches, switches_new);
shm_getg(actuator_desires, actuator_desires_new);
shm_getg(actuator_status, actuator_status_new);
if (actuator_desires_new.trigger_01 != actuator_desires.trigger_01) {
sendInt16(16, (actuator_desires_new.trigger_01));
actuator_desires.trigger_01 = actuator_desires_new.trigger_01;
}
if (actuator_desires_new.trigger_02 != actuator_desires.trigger_02) {
sendInt16(17, (actuator_desires_new.trigger_02));
actuator_desires.trigger_02 = actuator_desires_new.trigger_02;
}
if (actuator_desires_new.trigger_03 != actuator_desires.trigger_03) {
sendInt16(18, (actuator_desires_new.trigger_03));
actuator_desires.trigger_03 = actuator_desires_new.trigger_03;
}
if (actuator_desires_new.trigger_04 != actuator_desires.trigger_04) {
sendInt16(19, (actuator_desires_new.trigger_04));
actuator_desires.trigger_04 = actuator_desires_new.trigger_04;
}
if (actuator_desires_new.trigger_05 != actuator_desires.trigger_05) {
sendInt16(20, (actuator_desires_new.trigger_05));
actuator_desires.trigger_05 = actuator_desires_new.trigger_05;
}
if (actuator_desires_new.trigger_06 != actuator_desires.trigger_06) {
sendInt16(21, (actuator_desires_new.trigger_06));
actuator_desires.trigger_06 = actuator_desires_new.trigger_06;
}
if (actuator_desires_new.trigger_07 != actuator_desires.trigger_07) {
sendInt16(22, (actuator_desires_new.trigger_07));
actuator_desires.trigger_07 = actuator_desires_new.trigger_07;
}
if (actuator_desires_new.trigger_08 != actuator_desires.trigger_08) {
sendInt16(23, (actuator_desires_new.trigger_08));
actuator_desires.trigger_08 = actuator_desires_new.trigger_08;
}
if (actuator_desires_new.trigger_09 != actuator_desires.trigger_09) {
sendInt16(24, (actuator_desires_new.trigger_09));
actuator_desires.trigger_09 = actuator_desires_new.trigger_09;
}
if (actuator_desires_new.trigger_10 != actuator_desires.trigger_10) {
sendInt16(25, (actuator_desires_new.trigger_10));
actuator_desires.trigger_10 = actuator_desires_new.trigger_10;
}
if (actuator_desires_new.trigger_11 != actuator_desires.trigger_11) {
sendInt16(26, (actuator_desires_new.trigger_11));
actuator_desires.trigger_11 = actuator_desires_new.trigger_11;
}
if (actuator_desires_new.trigger_12 != actuator_desires.trigger_12) {
sendInt16(27, (actuator_desires_new.trigger_12));
actuator_desires.trigger_12 = actuator_desires_new.trigger_12;
}
if (actuator_desires_new.trigger_13 != actuator_desires.trigger_13) {
sendInt16(28, (actuator_desires_new.trigger_13));
actuator_desires.trigger_13 = actuator_desires_new.trigger_13;
}
if (actuator_desires_new.trigger_14 != actuator_desires.trigger_14) {
sendInt16(29, (actuator_desires_new.trigger_14));
actuator_desires.trigger_14 = actuator_desires_new.trigger_14;
}
if (actuator_desires_new.motor_pwm_1 != actuator_desires.motor_pwm_1) {
sendInt16(30, (actuator_desires_new.motor_pwm_1));
actuator_desires.motor_pwm_1 = actuator_desires_new.motor_pwm_1;
}
if (actuator_desires_new.motor_pwm_2 != actuator_desires.motor_pwm_2) {
sendInt16(31, (actuator_desires_new.motor_pwm_2));
actuator_desires.motor_pwm_2 = actuator_desires_new.motor_pwm_2;
}
if (switches_new.soft_kill != switches.soft_kill) {
sendInt16(32, (switches_new.soft_kill));
switches.soft_kill = switches_new.soft_kill;
}
}
destroy_watcher(w);
}