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);
}
Him::Him(int id, const char* name, const char* path, bool debugSend, bool debugReceive)
: Device(id, name, path, 57600, debugSend, debugReceive) {
addPollBlock(new PollBlock(100000, 34, 26));
shm_getg(him_settings, him_settings);
shm_getg(him, him);
}
Actuators::Actuators(int id, const char* name, const char* path, bool debugSend, bool debugReceive)
: Device(id, name, path, 57600, debugSend, debugReceive) {
addPollBlock(new PollBlock(100000, 33, 5));
shm_getg(switches, switches);
shm_getg(actuator_desires, actuator_desires);
shm_getg(actuator_status, actuator_status);
}
Pod::Pod(int id, const char* name, const char* path, bool debugSend, bool debugReceive)
: Device(id, name, path, 57600, debugSend, debugReceive) {
addPollBlock(new PollBlock(100000, 16, 14));
shm_getg(switches, switches);
shm_getg(mission_start_switch, mission_start_switch);
shm_getg(merge_status, merge_status);
}
int main (int argc, char ** argv) {
w=windowcf_create(SPECTRUM_FFT_LENGTH);
wchA = windowcf_create(TRACK_LENGTH);
wchB = windowcf_create(TRACK_LENGTH);
wchC = windowcf_create(TRACK_LENGTH);
wchA_filtered = windowcf_create(TRACK_LENGTH);
wchB_filtered = windowcf_create(TRACK_LENGTH);
wchC_filtered = windowcf_create(TRACK_LENGTH);
wmag_buffer = windowf_create(TRACK_LENGTH);
nco = nco_crcf_create(LIQUID_NCO);
std::cout << "hydromath daemon is beginning..." << std::endl;
std::cout << "NOTE: if this is symhydromath you must pass in a matfile" << std::endl;
std::cout << argv[1] << std::endl;
shm_init();
shm_getg(hydrophones_results_track, shm_results_track);
shm_getg(hydrophones_results_spectrum, shm_results_spectrum);
//shm_results_track.tracked_ping_count=0;
shm_results_track.tracked_ping_time=0;
if (argc > 1) {
udp_init(argv[1]);
}
else {
udp_init("");
}
//std::thread track_thread(direction_loop);
//
//std::thread spectrum_thread(spectrum_loop);
track_sample_idx = 0;
while (loop(&spt) == 0) {
shm_getg(hydrophones_settings, shm_settings);
for (int i = 0; i < 3*CHANNEL_DEPTH; i+=3) {
windowcf_push(w, std::complex<float>(spt.data[i+1],0)); //This uses channel B
windowcf_push(wchA, std::complex<float>(spt.data[i],0));
windowcf_push(wchB, std::complex<float>(spt.data[i+1],0));
windowcf_push(wchC, std::complex<float>(spt.data[i+2],0));
}
current_sample_count+=CHANNEL_DEPTH;
do_spectrum();
do_track();
}
printf("loop done %li =ci\n",current_sample_count/CHANNEL_DEPTH);
return 0;
}
void draw_loop(GLFWwindow *window) {
while (!glfwWindowShouldClose(window)) {
// Timing needs to be maintained if using single buffered context.
/*timespec curr_time;
clock_gettime(CLOCK_REALTIME, &curr_time);
auto get_time_us = [] (const timespec &curr_time) {
return 1000000 * curr_time.tv_sec + curr_time.tv_nsec / 1000;
};
long elapsed = get_time_us(curr_time) - get_time_us(last_time);
float fps = 1000000. / elapsed;
last_time = curr_time;*/
shm_getg(kalman, kalman_g);
update_transformations();
handle_input(window);
render_all(cam);
glfwSwapBuffers(window);
glfwPollEvents();
/*clock_gettime(CLOCK_REALTIME, &curr_time);
elapsed = get_time_us(curr_time) - get_time_us(last_time);
float sleep_time_ms = (1000 / TARGET_FPS) - elapsed / 1000.;
printf("%f FPS, time taken is %lu us\n", fps, elapsed);
if (sleep_time_ms < 0) {
sleep_time_ms = 0;
}*/
}
}
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 Him::handleReadResponse(uint16_t address, uint8_t count, uint8_t* buffer) {
int i = 0;
bool him_settings_changed = false;
bool him_changed = false;
union {
double vDouble;
int vInt;
bool vBool;
};
(void)vDouble;
(void)vInt;
(void)vBool;
while (i < count) {
switch(address + i) {
case 16:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him_settings.heading_offset) {
// If first change on this group, resync
if (!him_settings_changed) {
shm_getg(him_settings, him_settings);
}
him_settings_changed = true;
him_settings.heading_offset = vDouble;
}
i += 2;
buffer += 4;
break;
case 18:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him_settings.pitchoffset) {
// If first change on this group, resync
if (!him_settings_changed) {
shm_getg(him_settings, him_settings);
}
him_settings_changed = true;
him_settings.pitchoffset = vDouble;
}
i += 2;
buffer += 4;
break;
case 20:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him_settings.rolloffset) {
// If first change on this group, resync
if (!him_settings_changed) {
shm_getg(him_settings, him_settings);
}
him_settings_changed = true;
him_settings.rolloffset = vDouble;
}
i += 2;
buffer += 4;
break;
case 22:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him_settings.xacceloffset) {
// If first change on this group, resync
if (!him_settings_changed) {
shm_getg(him_settings, him_settings);
}
him_settings_changed = true;
him_settings.xacceloffset = vDouble;
}
i += 2;
buffer += 4;
break;
case 24:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him_settings.yacceloffset) {
// If first change on this group, resync
if (!him_settings_changed) {
shm_getg(him_settings, him_settings);
}
him_settings_changed = true;
him_settings.yacceloffset = vDouble;
}
i += 2;
buffer += 4;
break;
case 26:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him_settings.zacceloffset) {
// If first change on this group, resync
if (!him_settings_changed) {
shm_getg(him_settings, him_settings);
}
him_settings_changed = true;
him_settings.zacceloffset = vDouble;
}
i += 2;
buffer += 4;
break;
case 28:
vInt = (int16FromBuffer(buffer));
if (vInt != him_settings.xcompoffset) {
// If first change on this group, resync
if (!him_settings_changed) {
shm_getg(him_settings, him_settings);
}
him_settings_changed = true;
him_settings.xcompoffset = vInt;
}
i += 1;
buffer += 2;
break;
case 29:
vInt = (int16FromBuffer(buffer));
if (vInt != him_settings.ycompoffset) {
// If first change on this group, resync
if (!him_settings_changed) {
shm_getg(him_settings, him_settings);
}
him_settings_changed = true;
him_settings.ycompoffset = vInt;
}
i += 1;
buffer += 2;
break;
case 30:
vInt = (int16FromBuffer(buffer));
if (vInt != him_settings.zcompoffset) {
// If first change on this group, resync
if (!him_settings_changed) {
shm_getg(him_settings, him_settings);
}
him_settings_changed = true;
him_settings.zcompoffset = vInt;
}
i += 1;
buffer += 2;
break;
case 34:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him.x_accel) {
// If first change on this group, resync
if (!him_changed) {
shm_getg(him, him);
}
him_changed = true;
him.x_accel = vDouble;
}
i += 2;
buffer += 4;
break;
case 36:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him.y_accel) {
// If first change on this group, resync
if (!him_changed) {
shm_getg(him, him);
}
him_changed = true;
him.y_accel = vDouble;
}
i += 2;
buffer += 4;
break;
case 38:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him.z_accel) {
// If first change on this group, resync
if (!him_changed) {
shm_getg(him, him);
}
him_changed = true;
him.z_accel = vDouble;
}
i += 2;
buffer += 4;
break;
case 40:
vDouble = -(floatFromBuffer(buffer));
if (vDouble != him.pitch_vel) {
// If first change on this group, resync
if (!him_changed) {
shm_getg(him, him);
}
him_changed = true;
him.pitch_vel = vDouble;
}
i += 2;
buffer += 4;
break;
case 42:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him.roll_vel) {
// If first change on this group, resync
if (!him_changed) {
shm_getg(him, him);
}
him_changed = true;
him.roll_vel = vDouble;
}
i += 2;
buffer += 4;
break;
case 44:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him.yaw_vel) {
// If first change on this group, resync
if (!him_changed) {
shm_getg(him, him);
}
him_changed = true;
him.yaw_vel = vDouble;
}
i += 2;
buffer += 4;
break;
case 46:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him.mag_x) {
// If first change on this group, resync
if (!him_changed) {
shm_getg(him, him);
}
him_changed = true;
him.mag_x = vDouble;
}
i += 2;
buffer += 4;
break;
case 48:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him.mag_y) {
// If first change on this group, resync
if (!him_changed) {
shm_getg(him, him);
}
him_changed = true;
him.mag_y = vDouble;
}
i += 2;
buffer += 4;
break;
case 50:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him.mag_z) {
// If first change on this group, resync
if (!him_changed) {
shm_getg(him, him);
}
him_changed = true;
him.mag_z = vDouble;
}
i += 2;
buffer += 4;
break;
case 52:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him.heading) {
// If first change on this group, resync
if (!him_changed) {
shm_getg(him, him);
}
him_changed = true;
him.heading = vDouble;
}
i += 2;
buffer += 4;
break;
case 54:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him.pitch) {
// If first change on this group, resync
if (!him_changed) {
shm_getg(him, him);
}
him_changed = true;
him.pitch = vDouble;
}
i += 2;
buffer += 4;
break;
case 56:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him.roll) {
// If first change on this group, resync
if (!him_changed) {
shm_getg(him, him);
}
him_changed = true;
him.roll = vDouble;
}
i += 2;
buffer += 4;
break;
case 58:
vDouble = (floatFromBuffer(buffer));
if (vDouble != him.gyro_temp) {
// If first change on this group, resync
if (!him_changed) {
shm_getg(him, him);
}
him_changed = true;
him.gyro_temp = vDouble;
}
i += 2;
buffer += 4;
break;
default:
++i;
buffer += 2;
}
}
if (him_changed) {
shm_setg(him, him);
}
}
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);
}
void Actuators::handleReadResponse(uint16_t address, uint8_t count, uint8_t* buffer) {
int i = 0;
bool switches_changed = false;
bool actuator_desires_changed = false;
bool actuator_status_changed = false;
union {
double vDouble;
int vInt;
bool vBool;
};
(void)vDouble;
(void)vInt;
(void)vBool;
while (i < count) {
switch(address + i) {
case 16:
vInt = (int16FromBuffer(buffer));
if (vInt != actuator_desires.trigger_01) {
// If first change on this group, resync
if (!actuator_desires_changed) {
shm_getg(actuator_desires, actuator_desires);
}
actuator_desires_changed = true;
actuator_desires.trigger_01 = vInt;
}
i += 1;
buffer += 2;
break;
case 17:
vInt = (int16FromBuffer(buffer));
if (vInt != actuator_desires.trigger_02) {
// If first change on this group, resync
if (!actuator_desires_changed) {
shm_getg(actuator_desires, actuator_desires);
}
actuator_desires_changed = true;
actuator_desires.trigger_02 = vInt;
}
i += 1;
buffer += 2;
break;
case 18:
vInt = (int16FromBuffer(buffer));
if (vInt != actuator_desires.trigger_03) {
// If first change on this group, resync
if (!actuator_desires_changed) {
shm_getg(actuator_desires, actuator_desires);
}
actuator_desires_changed = true;
actuator_desires.trigger_03 = vInt;
}
i += 1;
buffer += 2;
break;
case 19:
vInt = (int16FromBuffer(buffer));
if (vInt != actuator_desires.trigger_04) {
// If first change on this group, resync
if (!actuator_desires_changed) {
shm_getg(actuator_desires, actuator_desires);
}
actuator_desires_changed = true;
actuator_desires.trigger_04 = vInt;
}
i += 1;
buffer += 2;
break;
case 20:
vInt = (int16FromBuffer(buffer));
if (vInt != actuator_desires.trigger_05) {
// If first change on this group, resync
if (!actuator_desires_changed) {
shm_getg(actuator_desires, actuator_desires);
}
actuator_desires_changed = true;
actuator_desires.trigger_05 = vInt;
}
i += 1;
buffer += 2;
break;
case 21:
vInt = (int16FromBuffer(buffer));
if (vInt != actuator_desires.trigger_06) {
// If first change on this group, resync
if (!actuator_desires_changed) {
shm_getg(actuator_desires, actuator_desires);
}
actuator_desires_changed = true;
actuator_desires.trigger_06 = vInt;
}
i += 1;
buffer += 2;
break;
case 22:
vInt = (int16FromBuffer(buffer));
if (vInt != actuator_desires.trigger_07) {
// If first change on this group, resync
if (!actuator_desires_changed) {
shm_getg(actuator_desires, actuator_desires);
}
actuator_desires_changed = true;
actuator_desires.trigger_07 = vInt;
}
i += 1;
buffer += 2;
break;
case 23:
vInt = (int16FromBuffer(buffer));
if (vInt != actuator_desires.trigger_08) {
// If first change on this group, resync
if (!actuator_desires_changed) {
shm_getg(actuator_desires, actuator_desires);
}
actuator_desires_changed = true;
actuator_desires.trigger_08 = vInt;
}
i += 1;
buffer += 2;
break;
case 24:
vInt = (int16FromBuffer(buffer));
if (vInt != actuator_desires.trigger_09) {
// If first change on this group, resync
if (!actuator_desires_changed) {
shm_getg(actuator_desires, actuator_desires);
}
actuator_desires_changed = true;
actuator_desires.trigger_09 = vInt;
}
i += 1;
buffer += 2;
break;
case 25:
vInt = (int16FromBuffer(buffer));
if (vInt != actuator_desires.trigger_10) {
// If first change on this group, resync
if (!actuator_desires_changed) {
shm_getg(actuator_desires, actuator_desires);
}
actuator_desires_changed = true;
actuator_desires.trigger_10 = vInt;
}
i += 1;
buffer += 2;
break;
case 26:
vInt = (int16FromBuffer(buffer));
if (vInt != actuator_desires.trigger_11) {
// If first change on this group, resync
if (!actuator_desires_changed) {
shm_getg(actuator_desires, actuator_desires);
}
actuator_desires_changed = true;
actuator_desires.trigger_11 = vInt;
}
i += 1;
buffer += 2;
break;
case 27:
vInt = (int16FromBuffer(buffer));
if (vInt != actuator_desires.trigger_12) {
// If first change on this group, resync
if (!actuator_desires_changed) {
shm_getg(actuator_desires, actuator_desires);
}
actuator_desires_changed = true;
actuator_desires.trigger_12 = vInt;
}
i += 1;
buffer += 2;
break;
case 28:
vInt = (int16FromBuffer(buffer));
if (vInt != actuator_desires.trigger_13) {
// If first change on this group, resync
if (!actuator_desires_changed) {
shm_getg(actuator_desires, actuator_desires);
}
actuator_desires_changed = true;
actuator_desires.trigger_13 = vInt;
}
i += 1;
buffer += 2;
break;
case 29:
vInt = (int16FromBuffer(buffer));
if (vInt != actuator_desires.trigger_14) {
// If first change on this group, resync
if (!actuator_desires_changed) {
shm_getg(actuator_desires, actuator_desires);
}
actuator_desires_changed = true;
actuator_desires.trigger_14 = vInt;
}
i += 1;
buffer += 2;
break;
case 30:
vInt = (int16FromBuffer(buffer));
if (vInt != actuator_desires.motor_pwm_1) {
// If first change on this group, resync
if (!actuator_desires_changed) {
shm_getg(actuator_desires, actuator_desires);
}
actuator_desires_changed = true;
actuator_desires.motor_pwm_1 = vInt;
}
i += 1;
buffer += 2;
break;
case 31:
vInt = (int16FromBuffer(buffer));
if (vInt != actuator_desires.motor_pwm_2) {
// If first change on this group, resync
if (!actuator_desires_changed) {
shm_getg(actuator_desires, actuator_desires);
}
actuator_desires_changed = true;
actuator_desires.motor_pwm_2 = vInt;
}
i += 1;
buffer += 2;
break;
case 32:
vInt = (int16FromBuffer(buffer));
if (vInt != switches.soft_kill) {
// If first change on this group, resync
if (!switches_changed) {
shm_getg(switches, switches);
}
switches_changed = true;
switches.soft_kill = vInt;
}
i += 1;
buffer += 2;
break;
case 33:
vInt = (int16FromBuffer(buffer));
if (vInt != actuator_status.blown_fuses) {
// If first change on this group, resync
if (!actuator_status_changed) {
shm_getg(actuator_status, actuator_status);
}
actuator_status_changed = true;
actuator_status.blown_fuses = vInt;
}
i += 1;
buffer += 2;
break;
case 34:
vDouble = (floatFromBuffer(buffer));
if (vDouble != actuator_status.motor_current_1) {
// If first change on this group, resync
if (!actuator_status_changed) {
shm_getg(actuator_status, actuator_status);
}
actuator_status_changed = true;
actuator_status.motor_current_1 = vDouble;
}
i += 2;
buffer += 4;
break;
case 36:
vDouble = (floatFromBuffer(buffer));
if (vDouble != actuator_status.motor_current_2) {
// If first change on this group, resync
if (!actuator_status_changed) {
shm_getg(actuator_status, actuator_status);
}
actuator_status_changed = true;
actuator_status.motor_current_2 = vDouble;
}
i += 2;
buffer += 4;
break;
default:
++i;
buffer += 2;
}
}
if (switches_changed) {
shm_setg(switches, switches);
}
if (actuator_status_changed) {
shm_setg(actuator_status, actuator_status);
}
}
void des_thrusters::step(double delta)
{
bool softkilled;
shm_get(switches, soft_kill, softkilled);
shm_getg(settings_control, shm_settings);
if (softkilled || !shm_settings.enabled) {
f -= f;
t -= t;
return;
}
// Read PID settings & desires.
// (would switching to watchers improve performance?)
SHM_GET_PID(settings_heading, shm_hp, hp)
SHM_GET_PID(settings_pitch, shm_pp, pp)
SHM_GET_PID(settings_roll, shm_rp, rp)
SHM_GET_PID(settings_velx, shm_xp, xp)
SHM_GET_PID(settings_vely, shm_yp, yp)
SHM_GET_PID(settings_depth, shm_dp, dp)
shm_getg(desires, shm_desires);
// Read current orientation, velocity & position (in the model frame).
// Orientation quaternion in the model frame.
Eigen::Quaterniond qm = q * mtob_rq;
Eigen::Vector3d rph = quat_to_euler(qm);
// Component of the model quaternion about the z-axis (heading-only rotation).
Eigen::Quaterniond qh = euler_to_quat(rph[2], 0, 0);
// Velocity in heading modified world space.
Eigen::Vector3d vp = qh.conjugate() * v;
// Step the PID loops.
Eigen::Vector3d desires = quat_to_euler(euler_to_quat(shm_desires.heading * DEG_TO_RAD,
shm_desires.pitch * DEG_TO_RAD,
shm_desires.roll * DEG_TO_RAD)) * RAD_TO_DEG;
double ho = hp.step(delta, desires[2], rph[2] * RAD_TO_DEG);
double po = pp.step(delta, desires[1], rph[1] * RAD_TO_DEG);
double ro = rp.step(delta, desires[0], rph[0] * RAD_TO_DEG);
double xo = xp.step(delta, shm_desires.speed, vp[0]);
double yo = yp.step(delta, shm_desires.sway_speed, vp[1]);
double zo = dp.step(delta, shm_desires.depth, x[2]);
// f is in the heading modified world frame.
// t is in the model frame.
// We will work with f and b in the model frame until the end of this
// function.
f[0] = shm_settings.velx_active ? xo : 0;
f[1] = shm_settings.vely_active ? yo : 0;
f[2] = shm_settings.depth_active ? zo : 0;
f *= m;
Eigen::Vector3d w_in;
w_in[0] = shm_settings.roll_active ? ro : 0;
w_in[1] = shm_settings.pitch_active ? po : 0;
w_in[2] = shm_settings.heading_active ? ho : 0;
// TODO Avoid this roundabout conversion from hpr frame
// to world frame and back to model frame.
Eigen::Quaterniond qhp = euler_to_quat(rph[2], rph[1], 0);
Eigen::Vector3d w = qm.conjugate() * (Eigen::Vector3d(0, 0, w_in[2]) +
qh * Eigen::Vector3d(0, w_in[1], 0) +
qhp * Eigen::Vector3d(w_in[0], 0, 0));
t = btom_rm * q.conjugate() * I * q * mtob_rm * w;
// Output diagnostic information. Shown by auv-control-helm.
SHM_PUT_PID(control_internal_heading, ho)
SHM_PUT_PID(control_internal_pitch, po)
SHM_PUT_PID(control_internal_roll, ro)
SHM_PUT_PID(control_internal_velx, xo)
SHM_PUT_PID(control_internal_vely, yo)
SHM_PUT_PID(control_internal_depth, zo)
// Subtract passive forces.
// (this implementation does not support discrimination!)
if (shm_settings.buoyancy_forces || shm_settings.drag_forces) {
// pff is in the body frame.
screw ps = pff();
f -= qh.conjugate() * q * ps.first;
t -= btom_rm * ps.second;
}
// Hyper-ellipsoid clamping. Sort of limiting to the maximum amount of
// energy the sub can output (e.g. can't move forward at full speed
// and pitch at full speed at the same time).
// Doesn't really account for real-world thruster configurations (e.g.
// it might be possible to move forward at full speed and ascend at
// full speed at the same time) but it's just an approximation.
for (int i = 0; i < 3; i++) {
f[i] /= axes[i];
}
for (int i = 0; i < 3; i++) {
t[i] /= axes[3 + i];
}
double m = f.dot(f) + t.dot(t);
if (m > 1) {
double sm = sqrt(m);
f /= sm;
t /= sm;
}
for (int i = 0; i < 3; i++) {
f[i] *= axes[i];
}
for (int i = 0; i < 3; i++) {
t[i] *= axes[3 + i];
}
// Regular min/max clamping.
clamp(f, fa, fb, 3);
clamp(t, ta, tb, 3);
f = q.conjugate() * qh * f;
t = mtob_rm * t;
}
void Pod::handleReadResponse(uint16_t address, uint8_t count, uint8_t* buffer) {
int i = 0;
bool switches_changed = false;
bool mission_start_switch_changed = false;
bool merge_status_changed = false;
union {
double vDouble;
int vInt;
bool vBool;
};
(void)vDouble;
(void)vInt;
(void)vBool;
while (i < count) {
switch(address + i) {
case 16:
vDouble = (floatFromBuffer(buffer));
if (vDouble != merge_status.total_current) {
// If first change on this group, resync
if (!merge_status_changed) {
shm_getg(merge_status, merge_status);
}
merge_status_changed = true;
merge_status.total_current = vDouble;
}
i += 2;
buffer += 4;
break;
case 18:
vDouble = (floatFromBuffer(buffer));
if (vDouble != merge_status.total_voltage) {
// If first change on this group, resync
if (!merge_status_changed) {
shm_getg(merge_status, merge_status);
}
merge_status_changed = true;
merge_status.total_voltage = vDouble;
}
i += 2;
buffer += 4;
break;
case 20:
vDouble = (floatFromBuffer(buffer));
if (vDouble != merge_status.current_starboard) {
// If first change on this group, resync
if (!merge_status_changed) {
shm_getg(merge_status, merge_status);
}
merge_status_changed = true;
merge_status.current_starboard = vDouble;
}
i += 2;
buffer += 4;
break;
case 22:
vDouble = (floatFromBuffer(buffer));
if (vDouble != merge_status.voltage_starboard) {
// If first change on this group, resync
if (!merge_status_changed) {
shm_getg(merge_status, merge_status);
}
merge_status_changed = true;
merge_status.voltage_starboard = vDouble;
}
i += 2;
buffer += 4;
break;
case 24:
vDouble = (floatFromBuffer(buffer));
if (vDouble != merge_status.current_port) {
// If first change on this group, resync
if (!merge_status_changed) {
shm_getg(merge_status, merge_status);
}
merge_status_changed = true;
merge_status.current_port = vDouble;
}
i += 2;
buffer += 4;
break;
case 26:
vDouble = (floatFromBuffer(buffer));
if (vDouble != merge_status.voltage_port) {
// If first change on this group, resync
if (!merge_status_changed) {
shm_getg(merge_status, merge_status);
}
merge_status_changed = true;
merge_status.voltage_port = vDouble;
}
i += 2;
buffer += 4;
break;
case 28:
vInt = (int16FromBuffer(buffer));
if (vInt != merge_status.mission_start) {
// If first change on this group, resync
if (!merge_status_changed) {
shm_getg(merge_status, merge_status);
}
merge_status_changed = true;
merge_status.mission_start = vInt;
}
i += 1;
buffer += 2;
break;
case 29:
vInt = unit_hard_kill(int16FromBuffer(buffer));
if (vInt != switches.hard_kill) {
// If first change on this group, resync
if (!switches_changed) {
shm_getg(switches, switches);
}
switches_changed = true;
switches.hard_kill = vInt;
}
i += 1;
buffer += 2;
break;
case 30:
vInt = (int16FromBuffer(buffer));
if (vInt != mission_start_switch.mission_light) {
// If first change on this group, resync
if (!mission_start_switch_changed) {
shm_getg(mission_start_switch, mission_start_switch);
}
mission_start_switch_changed = true;
mission_start_switch.mission_light = vInt;
}
i += 1;
buffer += 2;
break;
default:
++i;
buffer += 2;
}
}
if (switches_changed) {
shm_setg(switches, switches);
}
if (merge_status_changed) {
shm_setg(merge_status, merge_status);
}
}
