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