#include "vehicle.h" #include "thruster_curves.h" #include #include "control.h" Vehicle vehicle; float auto_heading_setpoint = 0.0; float auto_depth_setpoint = 0.0; float auto_cruise_setpoint = 0.0; float auto_cruise_lat_setpoint; float auto_cruise_use_bottom_lock; float latitude = 0.0; float array1[100]; /* 2 minutes */ float array2[50]; /* 30 minutes */ float array3[25]; /* 120 minutes */ float pitch_old[5]; float roll_old[5]; static int dcount1 = 0; static int dcount2 = 0; static int dcount3 = 0; extern int depth_hist_thread_timer; Vehicle::Vehicle() { /* the following are the top input channels */ arm.command_shoulder_channel_top = -1; arm.command_swing_channel_top = -1; arm.command_elbow_channel_top = -1; arm.command_pitch_channel_top = -1; arm.command_yaw_channel_top = -1; arm.arm_string = "\0"; arm.arm_enable = 0; // initially arm enable should be off /* the following are the top input channels */ camera.command_shoulder_channel_top = -1; camera.command_pan_channel_top = -1; camera.command_tilt_channel_top = -1; camera.cam_string = "\0"; camera.camera_enable = 0; // initially camera enable should be off autopilot.auto_pilot_string = "\0"; autopilot.lateral = 1; // hard coded in autopilot.vertical = 3; // hard coded in autopilot.auto_pilot_enable = 0; // initially autopilot enable should be off autopilot.headingSource = 0; bellypack_ch = 1; // hard coded in port_thruster = 0.0; stbd_thruster = 0.0; vert_thruster = 0.0; lat_thruster = 0.0; rpm_interval = 1; rpm_speed = 0; curr_vel = 0.0; curr_lat_vel = 0.0; for (int i = 0; i < 65; i++) mpl_digitals[i] ; for (int i = 0; i < 129; i++) gesout_digitals[i] ; for (int i = 0; i < 16; i++) gesdac_output[i] = 0; // Initialize teleos variable teleos.kf1_foreaft = 925920; teleos.kf1_turns = 925920; teleos.kf1_lateral = 15840; teleos.kf1_vertical = 26400; teleos.kf2_foreaft = 1339560; teleos.kf2_turns = 1339560; teleos.kf2_lateral = 78400; teleos.kf2_vertical = 108900; teleos.db_foreaft = 0.5; teleos.db_turns = 0.5 ; teleos.db_lateral = 0.5; teleos.db_vertical = 0.5; teleos.st_foreaft = 0; teleos.st_turns = 0; teleos.st_lateral = 0.25; teleos.st_vertical = 0.25; teleos.curve_mode = 1; } Vehicle::~Vehicle() { } char* Vehicle::append_cam_string() { snprintf(camera.camStr, 512, "%d|%d|%d|%d|%d|%d|%d|%d|%d|%d|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%d|%5.4f|%5.4f|%5.4f\n", SET_CAM, camera.shoulder_servo_channel_bot,camera.pan_servo_channel_bot,camera.tilt_servo_channel_bot, camera.sin_shoulder_channel, camera.cos_shoulder_channel, camera.sin_pan_channel, camera.cos_pan_channel, camera.sin_tilt_channel, camera.cos_tilt_channel, camera.p_shoulder, camera.d_shoulder, camera.i_shoulder, camera.p_pan, camera.d_pan, camera.i_pan, camera.p_tilt, camera.d_tilt, camera.i_tilt, camera.camera_enable, camera.offset_shoulder,camera.offset_pan,camera.offset_tilt); return camera.camStr; } // Called by send all data down on start up //Not called by commands.c to update the vehicle char* Vehicle::append_auto_string() { snprintf(autopilot.autoPilotStr, 256, "%d|%d|%d|%d|%d|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%5.4f|%d|%d|%d|%d\n", SET_PILOT, autopilot.headingSource, autopilot.depthTransducer, autopilot.AltitudeSource, autopilot.dvlInput, autopilot.p_autoHead, autopilot.i_autoHead, autopilot.d_autoHead, autopilot.p_autoDepth, autopilot.i_autoDepth, autopilot.d_autoDepth, autopilot.p_autoAlt, autopilot.i_autoAlt, autopilot.d_autoAlt, autopilot.p_autoCruise, autopilot.i_autoCruise, autopilot.d_autoCruise, autopilot.portThruster_ch, autopilot.STBDThruster_ch, autopilot.vertThruster_ch, autopilot.latThruster_ch); return autopilot.autoPilotStr; } char* Vehicle::append_arm_string() { snprintf(arm.armStr, 512, "%d|%d|%d|%5.4f|%5.4f|%5.4f|%5.4f|%d|%d|%5.4f|%5.4f|%5.4f|%5.4f|%d|%d|%5.4f|%5.4f|%5.4f|%5.4f|%d|%d|%5.4f|%5.4f|%5.4f|%5.4f|%d|%d|%5.4f|%5.4f|%5.4f|%5.4f|%d|%d|%d|%d|%d\n", SET_ARM, arm.sin_shoulder_channel, arm.cos_shoulder_channel, arm.offset_shoulder, arm.p_shoulder, arm.d_shoulder, arm.i_shoulder, arm.sin_swing_channel, arm.cos_swing_channel, arm.offset_swing, arm.p_swing, arm.d_swing, arm.i_swing, arm.sin_elbow_channel, arm.cos_elbow_channel, arm.offset_elbow, arm.p_elbow, arm.d_elbow, arm.i_elbow, arm.sin_pitch_channel, arm.cos_pitch_channel, arm.offset_pitch, arm.p_pitch, arm.d_pitch, arm.i_pitch, arm.sin_yaw_channel, arm.cos_yaw_channel, arm.offset_yaw, arm.p_yaw, arm.d_yaw, arm.i_yaw, arm.shoulder_servo_channel_bot, arm.swing_servo_channel_bot, arm.elbow_servo_channel_bot, arm.pitch_servo_channel_bot, arm.yaw_servo_channel_bot); return arm.armStr; } int Vehicle::update_vehicle_thruster(int index, int raw) { // The raw value here is still an integer raw // This contants (PORT, ...) are defined in vehicle.h // This array will be sent to logging string and vision string // inside telemetry_string.cpp // The gesdac are monitored by gesadc input 49 through 64 gesdac_output[index-49] = (raw * 10.0)/4095 - 5.00; switch (index) { case PORT: port_thruster = (raw * 10)/4095 - 5.00; break; case STBD: stbd_thruster = (raw * 10)/4095 - 5.00; break; case VERTICAL: vert_thruster = (raw * 10)/4095 - 5.00; break; case LATERAL: lat_thruster = (raw * 10)/4095 - 5.00; break; default: break; } return 1; } int Vehicle::set_curr_pitch(float pitch) { float change; if (pitch > 12 || pitch < -12) return -1; pitch_old[4] = pitch_old[3]; pitch_old[3] = pitch_old[2]; pitch_old[2] = pitch_old[1]; pitch_old[1] = pitch_old[0]; pitch_old[0] = pitch; curr_pitch = (pitch_old[4] + pitch_old[3] + pitch_old[2] + pitch_old[1] + pitch_old[0])/ 5; return 1; } int Vehicle::set_curr_roll(float roll) { float change; if (roll > 12 || roll < -12) return -1; roll_old[4] = roll_old[3]; roll_old[3] = roll_old[2]; roll_old[2] = roll_old[1]; roll_old[1] = roll_old[0]; roll_old[0] = roll; curr_roll = (roll_old[4] + roll_old[3] + roll_old[2] + roll_old[1] + roll_old[0])/ 5; return 1; } /*************************************************************** * Thread to update the depth history arrays * * * ****************************************************************/ void *depth_routine(void *args) { int count1 = 0; int count2 = 0; int count3 = 0; while(1){ if (count1 >= 2) { // every 2 seconds vehicle.update_depth_hist(1); count1 = 0; } if (count2 >= 30) { // every 30 seconds vehicle.update_depth_hist(2); count2 = 0; } if (count3 >= 120) { // every 2 minutes vehicle.update_depth_hist(3); count3 = 0; } sleep(1); count1++; count2++; count3++; if(depth_hist_thread_timer < 1000) depth_hist_thread_timer++; } return NULL; // never return, just to keep compiler happy } void Vehicle::update_depth_hist(int which_array) { switch(which_array) { case 1: if (dcount1 >= 100) dcount1 = 0; array1[dcount1++] = vehicle.curr_depth; break; case 2: if (dcount2 >= 50) dcount2 = 0; array2[dcount2++] = vehicle.curr_depth; break; case 3: if (dcount3 >= 25) dcount3 = 0; array3[dcount3++] = vehicle.curr_depth; break; default: return; } } /* * This is going to be called by commands.c where "result" * is this hist[] */ bool Vehicle::get_depth_hist(int which_array, char hist[]) { int index; int max_count; int need_padding = 0; int ntimes = 0; float* array; char temp[128]; switch(which_array) { case 1: index = dcount1 - 1; array = array1; max_count = 100; break; case 2: index = dcount2 - 1; array = array2; max_count = 50; break; case 3: index = dcount3 - 1; array = array3; max_count = 25; break; default: return false; } bzero(hist, strlen(hist)); for (ntimes, index; index >= 0 && ntimes < 25; ++ntimes, --index) { snprintf(temp, 128, "%4.2f|", array[index]); strcat(hist, temp); } max_count--; for(ntimes; ntimes < 25; ntimes++) { snprintf(temp, 128, "%4.2f|", array[max_count]); max_count--; strcat(hist, temp); } //cout <<"depth hist : "<