/*Mainloop*/ void loop() { /*Auswertung des anstehenden Aufgaben im Scheduler*/ int i; unsigned char new_value = 0; unsigned char new_heading = 0; for (i = 0; i < MAX_TASKS; i++) { if (scheduler[i].task == NOTHING || get_current_millis() < scheduler[i].tv_msec) {continue;} switch (scheduler[i].task) { case NOTHING: break; case MEASURE: /*Fordert den als Parameter uebergebenen Sensor auf eine Messung zu starten und bestimmt den Lese-Zeitpunkt*/ srf[scheduler[i].param - SE0_ADDRESS].measure(); schedule_add((long)srf_speed[scheduler[i].param - SE0_ADDRESS], READ_MEASURE, scheduler[i].param); scheduler[i].task = NOTHING; break; case READ_MEASURE: { unsigned short index = scheduler[i].param - SE0_ADDRESS; /*Liest die Messdaten des als Parameter uebergebenen Sensors aus und veranlasst erneute Messung*/ srf[index].read_it(get_current_millis()); schedule_add((long)srf[index].get_delay(), MEASURE, scheduler[i].param); if(state == HOLD_STILL || state == MEASURE_ENVIRONMENT || state == GET_ANCHOR) { nvalue[index] = 1; } /*if (state == HOLD_STILL || state == HEAD_TO_MIDDLE || state = HTM_DELAY) { nvalue2[scheduler[i].param - SE0_ADDRESS] = 1; }*/ new_value = scheduler[i].param; /*Korrektur der Messwerte bei Schieflage des Copters bei glaubhaften Winkeln*/ if ((index == 0 || index == 1) && (abs(current_roll_rad) < MAX_ROLL_ANGLE)) srf[index].set_mean((unsigned short)(srf[index].get_mean()*cos(current_roll_rad))); else if ((index == 2 || index == 3) && (abs(current_pitch_rad) < MAX_PITCH_ANGLE)) srf[index].set_mean((unsigned short)(srf[index].get_mean()*cos(current_roll_rad))); /*Korrektur der Messwerte, wenn sich ein Sensor im Nahbereich (< SE_MIN_DISTANCE) befindet*/ static unsigned char rm_state[SE_COUNT]; static short rm_min[SE_COUNT]; if (state != IDLE && state != INIT && state != DELAY) { if (rm_state[index] == 0 && srf[index].get_mean() < SE_MIN_DISTANCE) { rm_state[index] = 1; if (index == 0 || index == 2) rm_min[index] = srf[index+1].get_mean() - SE_MIN_DIFF; else rm_min[index] = srf[index-1].get_mean() - SE_MIN_DIFF; srf[index].set_mean(SE_MIN); } else if (rm_state[index] == 1) { if ((index == 0 || index == 2) && srf[index+1].get_mean() < rm_min[index]) { rm_state[index] = 0; } else if ((index == 1 || index == 3) && srf[index-1].get_mean() < rm_min[index]) { rm_state[index] = 0; } else { srf[index].set_mean(SE_MIN); } } } if (rm_state[0] == 1 && rm_state[1] == 1) {rm_state[0] = 0; rm_state[1] = 0;} if (rm_state[2] == 1 && rm_state[3] == 1) {rm_state[2] = 0; rm_state[3] = 0;} if (state == HOLD_STILL || state == GET_ANCHOR || state == HTM_DELAY) { if (nvalue2[0] == 1 && nvalue2[1] == 1 && nvalue2[2] == 1 && nvalue2[3] == 1) { slam_insert_v((srf[0].get_cm_per_s() + srf[1].get_cm_per_s())/2,(srf[2].get_cm_per_s() + srf[3].get_cm_per_s())/2,current_heading,get_current_millis()); nvalue2[0] = 0; nvalue2[1] = 0; nvalue2[2] = 0; nvalue2[3] = 0; } } /*Speichert den gelesenen Messwert in der entsprechenden Log-Datei*/ #if LOG > 0 FILE *fd; if (scheduler[i].param == SE0_ADDRESS) fd = fd_112; else if (scheduler[i].param == SE1_ADDRESS) fd = fd_113; else if (scheduler[i].param == SE2_ADDRESS) fd = fd_114; else fd = fd_115; fprintf(fd,"%lu\t%u\t%u\n", srf[scheduler[i].param - SE0_ADDRESS].get_msec(), srf[scheduler[i].param - SE0_ADDRESS].get_data(), srf[scheduler[i].param - SE0_ADDRESS].get_mean()); #endif scheduler[i].task = NOTHING; break; } case SAVE_LOG: /*Sichert die bisher geloggten Daten*/ #if LOG > 0 fclose(fd_112); fclose(fd_113); fclose(fd_114); fclose(fd_115); fclose(fd_data); char tmp_dir[32]; strcpy(tmp_dir,log_dir); fd_112 = fopen(strcat(tmp_dir,"/112"), "a"); strcpy(tmp_dir,log_dir); fd_113 = fopen(strcat(tmp_dir,"/113"), "a"); strcpy(tmp_dir,log_dir); fd_114 = fopen(strcat(tmp_dir,"/114"), "a"); strcpy(tmp_dir,log_dir); fd_115 = fopen(strcat(tmp_dir,"/115"), "a"); strcpy(tmp_dir,log_dir); fd_data= fopen(strcat(tmp_dir,"/data"),"a"); strcpy(tmp_dir,log_dir); schedule_add(LOG_SPEED,SAVE_LOG,0); #endif scheduler[i].task = NOTHING; break; case CHECK_STILL: /*Prueft auf Stillstand des Copters*/ if (still && state == HOLD_STILL) { if (scheduler[i].param < CHECK_STILL_COUNT) { schedule_add(CHECK_STILL_SPEED,CHECK_STILL,scheduler[i].param+1); } else { state = MEASURE_ENVIRONMENT; still = 0; schedule_add(CHECK_STILL_SPEED,CHECK_STILL, 0); } } else { schedule_add(CHECK_STILL_SPEED,CHECK_STILL,0); } scheduler[i].task = NOTHING; break; case CHANGE_STATE: /*Ändert den aktuellen Status zum als Parameter übergebenen Status*/ state = (states)scheduler[i].param; if ((states)scheduler[i].param == HOLD_STILL) hs_state = 0; scheduler[i].task = NOTHING; break; case SHOW_ME: { /*Schreibt Messwerte und Neigungswinkel auf das Terminal*/ char st[16]; switch (state) { case INIT: sprintf(st, "INIT"); break; case MEASURE_ENVIRONMENT: sprintf(st, "ME"); break; case IDLE: sprintf(st, "IDLE"); break; case HOLD_STILL: sprintf(st, "HOLD_STILL"); break; case GET_ALIGNMENT: sprintf(st, "GET_ALIGNMENT"); break; case GET_ANCHOR: sprintf(st, "GET_ANCHOR"); break; case DELAY: sprintf(st, "DELAY"); break; default: sprintf(st, "???"); break; } if (roll > 9 || roll < 0) printf("roll: %d\tpitch: %d\tyaw: %d\theading: %d\tdhead: %d\tSE0: %d\tSE1: %d\tSE2: %d\tSE3:%d\tstate: %s\n", roll, pitch, yaw, current_heading, desired_heading, srf[0].get_mean(), srf[1].get_mean(), srf[2].get_mean(), srf[3].get_mean(),st); else printf("roll: %d\t\tpitch: %d\tyaw: %d\theading: %d\tdhead. %d\tSE0: %d\tSE1: %d\tSE2: %d\tSE3:%d\tstate: %s\n", roll, pitch, yaw, current_heading, desired_heading, srf[0].get_mean(), srf[1].get_mean(), srf[2].get_mean(), srf[3].get_mean(),st); schedule_add(100,SHOW_ME,0); scheduler[i].task = NOTHING; /*Mavlinkkommunikation mit QGroundcontrol (basierend auf Based on http://qgroundcontrol.org/dev/mavlink_linux_integration_tutorial)*/ mavlink_message_t msg; uint16_t len; uint8_t buf[MAVLINK_MAX_PACKET_LEN]; static int i; i++; if (i == 10) { i = 0; mavlink_msg_heartbeat_pack(0, 0, &msg, 0, 0, 0, 0, 0); len = mavlink_msg_to_send_buffer(buf, &msg); sendto(s, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); } mavlink_msg_debug_vect_pack(0,0,&msg,"DEBUG",0,roll,srf[0].get_mean(),srf[0].get_data()); len = mavlink_msg_to_send_buffer(buf, &msg); sendto(s, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); break; } default: if(WARNINGS){perror("LOOP: Unbekannte Aufgabe im Scheduler.");} break; } } /*Auswertung der Daten, die an der seriellen Schnittstelle anliegen.*/ struct timeval tv; tv.tv_sec = 0; tv.tv_usec = 0; /*Prueft, ob Daten an der seriellen Schnittstelle anliegen*/ FD_ZERO(&tty_fdset); FD_SET(tty_fd, &tty_fdset); if (select(tty_fd+1, &tty_fdset, NULL, NULL, &tv) == -1) {perror("LOOP: select() fehlgeschlagen"); exit(1);} /*Liest ein einzelnes Byte von der seriellen Schnittstelle und prueft, ob ein Mavlinkpaket vervollstaendigt wurde*/ if (FD_ISSET(tty_fd, &tty_fdset)) { static mavlink_message_t msg; static mavlink_status_t status; char c[1]; if (read(tty_fd,c,1) == -1) {if(WARNINGS){perror("LOOP: Fehler beim Lesen aus " TTY_DEVICE);}} #if DEBUG_LEVEL > 2 printf("%#x\n",c[0]); #endif if (mavlink_parse_char(MAVLINK_COMM_0,(uint8_t) *c, &msg, &status)) { #if DEBUG_LEVEL > 1 printf("%u Mavlinkpaket empfangen: %d\n", get_current_millis(), msg.msgid); #endif //printf("%lu Mavlinkpaket empfangen: %d\n", get_current_millis(), msg.msgid); switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: /*Beim Empfang des ersten Heartbeats wird zunaechst ein eventuell vorhandener Datenstream gekuendigt und ein neuer Datenstream abonnemiert*/ mavlink_heartbeat_t hb; mavlink_msg_heartbeat_decode(&msg,&hb); if (!first_heartbeat) { first_heartbeat = 1; request_data_stream(MAV_DATA_STREAM_ALL,0,0); } //if (state == IDLE) { if (state == IDLE && (hb.custom_mode == 13 /*EXT_CTRL*/ || desktop_build)) { std::cout << "State changed from IDLE to INIT.\n"; state = INIT; schedule_add(2000,CHANGE_STATE,(int)HOLD_STILL); request_data_stream(MAV_DATA_STREAM_EXTRA2/*VFR_HUD*/,DATA_STREAM_SPEED,1); init_state = 1; } break; case MAVLINK_MSG_ID_VFR_HUD: /*Speichert bei Empfang von HUD-Daten die aktuelle Ausrichtung des Copters*/ mavlink_vfr_hud_t hud_data; mavlink_msg_vfr_hud_decode(&msg, &hud_data); old_heading = current_heading; current_heading = hud_data.heading; new_heading = 1; break; case MAVLINK_MSG_ID_ATTITUDE: mavlink_attitude_t adata; mavlink_msg_attitude_decode(&msg,&adata); old_heading = current_heading; current_heading = ((short)lround(DEG(adata.yaw))+360)%360; current_pitch_rad = adata.pitch; current_roll_rad = adata.roll; new_heading = 1; break; case MAVLINK_MSG_ID_RAW_IMU: /*Speichert die Beschleunigungen auf der x- und y-Achse*/ mavlink_raw_imu_t raw_imu; mavlink_msg_raw_imu_decode(&msg,&raw_imu); xacc = raw_imu.xacc; yacc = raw_imu.yacc; //slam_insert_acc(xacc,yacc,current_heading,get_current_millis()); std::cout << "xacc: " << xacc << " yacc: " << yacc << "\n"; break; case MAVLINK_MSG_ID_RC_CHANNELS_RAW: /*Prüft, ob eine Umstellung auf externe Kontrolle erfolgt ist*/ /*if (state != IDLE) {break;} mavlink_rc_channels_raw_t rc; mavlink_msg_rc_channels_raw_decode(&msg,&rc); if (desktop_build) init_state = 1; rc.chan5_raw = MODE_SWITCH_RANGE_UP-1; if (init_state && rc.chan5_raw < MODE_SWITCH_RANGE_UP && rc.chan5_raw > MODE_SWITCH_RANGE_DOWN) { std::cout << "State changed from IDLE to INIT.\n"; schedule_add(0,CHANGE_STATE,(int)INIT); schedule_add(2000,CHANGE_STATE,(int)HOLD_STILL); request_data_stream(MAV_DATA_STREAM_RC_CHANNELS,0,0); request_data_stream(MAV_DATA_STREAM_EXTRA2,DATA_STREAM_SPEED,1); request_data_stream(MAV_DATA_STREAM_RAW_SENSORS,DATA_STREAM_SPEED,1); } if(rc.chan5_raw > MODE_SWITCH_RANGE_UP || rc.chan5_raw < MODE_SWITCH_RANGE_DOWN) { init_state = 1; }*/ break; default: break; } } } /*Berechnung der vorgeschlagenen Werte fuer roll, pitch und yaw an Hand der neuen Messwerte*/ if (state != INIT && !new_value && !new_heading) {/*Wenn keine neuen Daten vorhanden sind -> Abarbeitung überspringen*/return;} switch (state) { case IDLE: /*Wartet auf Aktivierung der externen Kontrolle*/ break; case INIT: std::cout << "INIT\n"; /*Initialisiert Abarbeitung, wenn externe Kontrolle zum ersten Mal aktiviert wurde*/ schedule_add(5000,CHECK_STILL,0); #if LOG > 0 schedule_add(LOG_SPEED,SAVE_LOG,0); //schedule_add(SLAM_SPEED,SAVE_SLAM,0); #endif for (int i = 0; i < SE_COUNT; i++) { if ((int)pow(2,i) & ENABLED_SENSORS) { schedule_add(0, MEASURE, SE0_ADDRESS + i); } } slam_init(current_heading,get_current_millis()); state = DELAY; break; case HOLD_STILL: if(1) { static short set_point[SE_COUNT]; if (hs_state == 0) { hs_state = 1; set_point[0] = (srf[0].get_mean() + srf[1].get_mean())/2; set_point[1] = set_point[0]; set_point[2] = (srf[2].get_mean() + srf[3].get_mean())/2; set_point[3] = set_point[2]; //for (int i = 0; i < SE_COUNT; i++) { // set_point[i] = srf[i].get_mean(); // std::cout << "Set_Point " << i << ":" << set_point[i] << "\n"; //} /*for (int i = 0; i < SE_COUNT; i++) { if (set_point[i] < 100 || set_point[i] > 0) { if (i == 0 || i == 2) { set_point[i+1] -= (100 - set_point[i]); } else { set_point[i-1] -= (100 - set_point[i]); } set_point[i] = 100; } }*/ for (int i = 0; i < SE_COUNT; i++) { std::cout << "Set_Point " << i << ":" << set_point[i] << "\n"; } } /*Veranlasst den Copter auf Grundlage der Änderungen der Messwerte still zu stehen*/ if (!new_value) break; if (nvalue[0] == 1 && nvalue[1] == 1) { if (srf[0].get_mean() == SE_MIN) roll = between<short>(pid_roll.get(0 - srf[1].get_mean() + set_point[1],srf[1].get_msec_diff()),-max_roll,max_roll); else if (srf[1].get_mean() == SE_MIN) roll = between<short>(pid_roll.get(srf[0].get_mean() - set_point[0],srf[0].get_msec_diff()),-max_roll,max_roll); else roll = between<short>(pid_roll.get(((srf[0].get_mean() - set_point[0]) - (srf[1].get_mean() - set_point[1]))/2,(srf[0].get_msec_diff() + srf[1].get_msec_diff())/2),-max_roll,max_roll); nvalue[0] = 0; nvalue[1] = 0; } if (nvalue[2] == 1 && nvalue[3] == 1) { if (srf[2].get_mean() == SE_MIN) pitch = between<short>(pid_pitch.get(0 - srf[3].get_mean() + set_point[3],srf[3].get_msec_diff()),-max_pitch,max_pitch); else if (srf[3].get_mean() == SE_MIN) pitch = between<short>(pid_pitch.get(srf[2].get_mean() - set_point[2],srf[2].get_msec_diff()),-max_pitch,max_pitch); else pitch = between<short>(pid_pitch.get(((srf[2].get_mean() - set_point[2]) - (srf[3].get_mean() - set_point[3]))/2,(srf[2].get_msec_diff() + srf[3].get_msec_diff())/2),-max_pitch,max_pitch); nvalue[2] = 0; nvalue[3] = 0; } yaw = 0; send_ext_ctrl(); /*Prüfung auf Stillstand*/ if (abs(roll) <= STILL_FAK_ROLL*max_roll/100 && abs(pitch) <= STILL_FAK_PITCH*max_pitch/100) { //still = 1; still = 0; /*FIXME*/ } else { still = 0; } } break; case MEASURE_ENVIRONMENT: /*Vermisst die Umgebung durch eine Drehung um 360/SE_COUNT Grad*/ if (breakpoint == 1) {state = HOLD_STILL; hs_state= 0; break;} if (abs(xacc) > HOLD_STILL_MAX_XACC || abs(yacc) > HOLD_STILL_MAX_YACC) {/*Wurde der Copter zu stark bewegt: Abbruch*/state = HOLD_STILL; hs_state = 0; std::cout << "State changed to HOLD_STILL\n"; break;} if (first_heading == -1) { /*Speichert die anfaengliche Ausrichtung und veranlasst den Copter sich zu drehen*/ std::cout << "State changed to MEASURE_ENVIRONMENT\n"; first_heading = current_heading; roll = 0; pitch = 0; rotation = 0; yaw = rotation_angle_sign*CONST_YAW; send_ext_ctrl(); } //if (new_value) slam_insert_measurement(srf[new_value-SE0_ADDRESS].get_mean(),(current_heading + (360/SE_COUNT)*(new_value-SE0_ADDRESS))%360); /*Berechnung der aktuellen Drehung*/ if (new_heading) { if (abs(current_heading - old_heading) > 180/SE_COUNT) rotation += current_heading - old_heading - sign(current_heading - old_heading)*360; else rotation += current_heading - old_heading; for(int i = 0; i < SE_COUNT; i++) { if(nvalue[i] == 1) { env[(current_heading + srf[i].get_alignment()*(360/SE_COUNT))%360] = srf[i].get_mean(); nvalue[i] = 0; } } //if (abs(rotation) >= 360/SE_COUNT) { if (abs(rotation) >= rotation_angle) { /*Copter hat sich ausreichend gedreht, Abbruch der Vermessung*/ //state = HEAD_TO_MIDDLE; state = GET_ALIGNMENT; roll = 0; pitch = 0; yaw = 0; send_ext_ctrl(); rotation_angle_sign = sign(rotation); } } break; case GET_ALIGNMENT: { /*Bestimme für jeden Sensor die Minimalentfernung der letzten Drehung*/ /*short min_v[SE_COUNT]; short min_h[SE_COUNT]; for (int i = 0; i < SE_COUNT; i++) { min_v[i] = 0; min_h[i] = 0; }*/ short min_v = 0; for (int i = 0; i < rotation_angle; i++) { if (min_v < env[(first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360]) { min_v = env[(first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360]; desired_heading = (first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360; } /*for (int j = 0; j < SE_COUNT; j++) { if (min_v[j] < env[(first_heading + rotation_angle_sign*i + j*360/SE_COUNT + 360)%360]) { min_v[j] = env[(first_heading + rotation_angle_sign*i + j*360/SE_COUNT + 360)%360]; min_h[j] = (first_heading + rotation_angle_sign*i + 360)%360; } }*/ } //desired_heading = round((float)(min_h[0]+min_h[1]+min_h[2]+min_h[3])/4); /*Bestimmung der künftigen Drehrichtung*/ for (int k = 0; k < rotation_angle/2; k++) { /*Wenn Minimum in der ersten Hälfte der Drehung gefunden wurde, Wechsel der Drehrichtung bzw. Abbruch*/ if (desired_heading == (first_heading+rotation_angle_sign*k)%360) { rotation_angle_sign *= -1; if (init_state == 2) init_state = 3; break; } } /*Cleanup*/ first_heading = -1; yaw = 0; pid_roll.reset(); pid_pitch.reset(); for(int i = 0; i < 360; i++) env[i] = 0; if (init_state == 1) init_state = 2; if (init_state == 3) { state = GET_ANCHOR; pid_yaw.reset(); pid_yaw.set(HTM_YAW_KP,HTM_YAW_TN,HTM_YAW_TV); pid_yaw.set_target(0); roll = 0; pitch = 0; yaw = 0; std::cout << "State changed to GET_ANCHOR\n"; tv_old_heading = get_current_millis(); } else { state = HOLD_STILL; hs_state = 0; } break; } case GET_ANCHOR: /*Copter versucht sich stabil auf ANCHOR_DISTANCE und desired_heading zu stellen*/ if (new_heading) { short heading_diff; /*Berechnung der Differenz zwischen aktueller Ausrichtung und gewünschter Ausrichtung*/ if (abs(desired_heading - current_heading) > 180) { heading_diff = desired_heading - current_heading - sign(desired_heading - current_heading)*360; } else { heading_diff = desired_heading - current_heading; } yaw = pid_yaw.get(heading_diff,get_current_millis() - tv_old_heading); tv_old_heading = get_current_millis(); } if (new_value) { roll = 0; pitch = 0; if (nvalue[align_se] == 1) { if (align_se == 0) roll = between<short>(pid_roll_anc.get(srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_roll, max_roll); else if (align_se == 1) roll = between<short>(pid_roll_anc.get((-1)*srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_roll, max_roll); else if (align_se == 2) pitch = between<short>(pid_pitch_anc.get(srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_pitch, max_pitch); else if (align_se == 3) pitch = between<short>(pid_pitch_anc.get((-1)*srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_pitch, max_pitch); nvalue[align_se] = 0; } if (srf[0].get_mean() < 30) roll = 1000; else if (srf[1].get_mean() < 30) roll = -1000; else if (srf[2].get_mean() < 30) pitch = 1000; else if (srf[3].get_mean() < 30) pitch = -1000; if (abs(srf[0].get_mean() - ANCHOR_DISTANCE) < 5) { for (int i = 0; i < SE_COUNT; i++) nvalue[i] = 0; /*state = MOVE_BACKWARD*/ /*TODO Neuausrichtung des Copters?*/ } } if (breakpoint == 3) {roll = 0; pitch = 0; send_ext_ctrl();} else if (breakpoint != 2) {send_ext_ctrl();} break; case DELAY: break; case HTM_DELAY: break; default: break; } #if LOG > 0 fprintf(fd_data,"%lu\t%d\t%d\t%d\t%d\t%d\t%f\t%f\n", (unsigned long)get_current_millis(), roll, pitch, yaw, current_heading, state, current_roll_rad, current_pitch_rad); #endif }
/* Updates the viewpoint according to the object being tracked */ static bool camTrackCamera() { PROPULSION_STATS *psPropStats; DROID *psDroid; bool bFlying = false; /* Most importantly - see if the target we're tracking is dead! */ if (trackingCamera.target->died) { return(false); } /* Update the acceleration,velocity and position of the camera for movement */ updateCameraAcceleration(CAM_ALL); updateCameraVelocity(CAM_ALL); updateCameraPosition(CAM_ALL); /* Update the acceleration,velocity and rotation of the camera for rotation */ /* You can track roll as well (z axis) but it makes you ill and looks like a flight sim, so for now just pitch and orientation */ if (trackingCamera.target->type == OBJ_DROID) { psDroid = (DROID *)trackingCamera.target; psPropStats = asPropulsionStats + psDroid->asBits[COMP_PROPULSION].nStat; if (psPropStats->propulsionType == PROPULSION_TYPE_LIFT) { bFlying = true; } } if (bRadarAllign || trackingCamera.target->type == OBJ_DROID) { if (bFlying) { updateCameraRotationAcceleration(CAM_ALL); } else { updateCameraRotationAcceleration(CAM_X_AND_Y); } } if (bFlying) { updateCameraRotationVelocity(CAM_ALL); updateCameraRotationPosition(CAM_ALL); } else { updateCameraRotationVelocity(CAM_X_AND_Y); updateCameraRotationPosition(CAM_X_AND_Y); } /* Update the position that's now stored in trackingCamera.position */ player.p.x = trackingCamera.position.x; player.p.y = trackingCamera.position.y; player.p.z = trackingCamera.position.z; /* Update the rotations that're now stored in trackingCamera.rotation */ player.r.x = trackingCamera.rotation.x; player.r.y = trackingCamera.rotation.y; player.r.z = trackingCamera.rotation.z; /* There's a minimum for this - especially when John's VTOL code lets them land vertically on cliffs */ if (player.r.x > DEG(360 + MAX_PLAYER_X_ANGLE)) { player.r.x = DEG(360 + MAX_PLAYER_X_ANGLE); } /* Clip the position to the edge of the map */ CheckScrollLimits(); /* Store away our last update as acceleration and velocity are all fn()/dt */ trackingCamera.lastUpdate = realTime; if (bFullInfo) { flushConsoleMessages(); if (trackingCamera.target->type == OBJ_DROID) { printDroidInfo((DROID *)trackingCamera.target); } } /* Switch off if we're jumping to a new location and we've got there */ if (getRadarTrackingStatus()) { /* This will ensure we come to a rest and terminate the tracking routine once we're close enough */ if (trackingCamera.velocity * trackingCamera.velocity + trackingCamera.acceleration * trackingCamera.acceleration < 1.f && trackingCamera.rotVel * trackingCamera.rotVel + trackingCamera.rotAccel * trackingCamera.rotAccel < 1.f) { setWarCamActive(false); } } return(true); }
/* Fire a weapon at something */ bool combFire(WEAPON *psWeap, BASE_OBJECT *psAttacker, BASE_OBJECT *psTarget, int weapon_slot) { WEAPON_STATS *psStats; UDWORD firePause; SDWORD longRange; int compIndex; CHECK_OBJECT(psAttacker); CHECK_OBJECT(psTarget); ASSERT(psWeap != nullptr, "Invalid weapon pointer"); /* Don't shoot if the weapon_slot of a vtol is empty */ if (psAttacker->type == OBJ_DROID && isVtolDroid(((DROID *)psAttacker)) && psWeap->usedAmmo >= getNumAttackRuns(((DROID *)psAttacker), weapon_slot)) { objTrace(psAttacker->id, "VTOL slot %d is empty", weapon_slot); return false; } /* Get the stats for the weapon */ compIndex = psWeap->nStat; ASSERT_OR_RETURN(false , compIndex < numWeaponStats, "Invalid range referenced for numWeaponStats, %d > %d", compIndex, numWeaponStats); psStats = asWeaponStats + compIndex; // check valid weapon/prop combination if (!validTarget(psAttacker, psTarget, weapon_slot)) { return false; } unsigned fireTime = gameTime - deltaGameTime + 1; // Can fire earliest at the start of the tick. // See if reloadable weapon. if (psStats->upgrade[psAttacker->player].reloadTime) { unsigned reloadTime = psWeap->lastFired + weaponReloadTime(psStats, psAttacker->player); if (psWeap->ammo == 0) // Out of ammo? { fireTime = std::max(fireTime, reloadTime); // Have to wait for weapon to reload before firing. if (gameTime < fireTime) { return false; } } if (reloadTime <= fireTime) { //reset the ammo level psWeap->ammo = psStats->upgrade[psAttacker->player].numRounds; } } /* See when the weapon last fired to control it's rate of fire */ firePause = weaponFirePause(psStats, psAttacker->player); firePause = std::max(firePause, 1u); // Don't shoot infinitely many shots at once. fireTime = std::max(fireTime, psWeap->lastFired + firePause); if (gameTime < fireTime) { /* Too soon to fire again */ return false; } if (psTarget->visible[psAttacker->player] != UBYTE_MAX) { // Can't see it - can't hit it objTrace(psAttacker->id, "combFire(%u[%s]->%u): Object has no indirect sight of target", psAttacker->id, getName(psStats), psTarget->id); return false; } /* Check we can hit the target */ bool tall = (psAttacker->type == OBJ_DROID && isVtolDroid((DROID *)psAttacker)) || (psAttacker->type == OBJ_STRUCTURE && ((STRUCTURE *)psAttacker)->pStructureType->height > 1); if (proj_Direct(psStats) && !lineOfFire(psAttacker, psTarget, weapon_slot, tall)) { // Can't see the target - can't hit it with direct fire objTrace(psAttacker->id, "combFire(%u[%s]->%u): No direct line of sight to target", psAttacker->id, objInfo(psAttacker), psTarget->id); return false; } Vector3i deltaPos = psTarget->pos - psAttacker->pos; // if the turret doesn't turn, check if the attacker is in alignment with the target if (psAttacker->type == OBJ_DROID && !psStats->rotate) { uint16_t targetDir = iAtan2(deltaPos.xy); int dirDiff = abs(angleDelta(targetDir - psAttacker->rot.direction)); if (dirDiff > FIXED_TURRET_DIR) { return false; } } /* Now see if the target is in range - also check not too near */ int dist = iHypot(deltaPos.xy); longRange = proj_GetLongRange(psStats, psAttacker->player); int min_angle = 0; // Calculate angle for indirect shots if (!proj_Direct(psStats) && dist > 0) { min_angle = arcOfFire(psAttacker, psTarget, weapon_slot, true); // prevent extremely steep shots min_angle = std::min(min_angle, DEG(PROJ_ULTIMATE_PITCH)); // adjust maximum range of unit if forced to shoot very steep if (min_angle > DEG(PROJ_MAX_PITCH)) { //do not allow increase of max range though if (iSin(2 * min_angle) < iSin(2 * DEG(PROJ_MAX_PITCH))) // If PROJ_MAX_PITCH == 45, then always iSin(2*min_angle) <= iSin(2*DEG(PROJ_MAX_PITCH)), and the test is redundant. { longRange = longRange * iSin(2 * min_angle) / iSin(2 * DEG(PROJ_MAX_PITCH)); } } } int baseHitChance = 0; if (dist <= longRange && dist >= psStats->upgrade[psAttacker->player].minRange) { // get weapon chance to hit in the long range baseHitChance = weaponLongHit(psStats, psAttacker->player); // adapt for height adjusted artillery shots if (min_angle > DEG(PROJ_MAX_PITCH)) { baseHitChance = baseHitChance * iCos(min_angle) / iCos(DEG(PROJ_MAX_PITCH)); } } else { /* Out of range */ objTrace(psAttacker->id, "combFire(%u[%s]->%u): Out of range", psAttacker->id, getName(psStats), psTarget->id); return false; } // apply experience accuracy modifiers to the base //hit chance, not to the final hit chance int resultHitChance = baseHitChance; // add the attacker's experience if (psAttacker->type == OBJ_DROID) { SDWORD level = getDroidEffectiveLevel((DROID *) psAttacker); // increase total accuracy by EXP_ACCURACY_BONUS % for each experience level resultHitChance += EXP_ACCURACY_BONUS * level * baseHitChance / 100; } // subtract the defender's experience if (psTarget->type == OBJ_DROID) { SDWORD level = getDroidEffectiveLevel((DROID *) psTarget); // decrease weapon accuracy by EXP_ACCURACY_BONUS % for each experience level resultHitChance -= EXP_ACCURACY_BONUS * level * baseHitChance / 100; } if (psAttacker->type == OBJ_DROID && ((DROID *)psAttacker)->sMove.Status != MOVEINACTIVE && !psStats->fireOnMove) { return false; // Can't fire while moving } /* -------!!! From that point we are sure that we are firing !!!------- */ /* note when the weapon fired */ psWeap->lastFired = fireTime; /* reduce ammo if salvo */ if (psStats->upgrade[psAttacker->player].reloadTime) { psWeap->ammo--; } // increment the shots counter psWeap->shotsFired++; // predicted X,Y offset per sec Vector3i predict = psTarget->pos; // Target prediction if (isDroid(psTarget) && castDroid(psTarget)->sMove.bumpTime == 0) { DROID *psDroid = castDroid(psTarget); int32_t flightTime; if (proj_Direct(psStats) || dist <= psStats->upgrade[psAttacker->player].minRange) { flightTime = dist * GAME_TICKS_PER_SEC / psStats->flightSpeed; } else { int32_t vXY, vZ; // Unused, we just want the flight time. flightTime = projCalcIndirectVelocities(dist, deltaPos.z, psStats->flightSpeed, &vXY, &vZ, min_angle); } if (psTarget->lastHitWeapon == WSC_EMP) { int empTime = EMP_DISABLE_TIME - (gameTime - psTarget->timeLastHit); CLIP(empTime, 0, EMP_DISABLE_TIME); if (empTime >= EMP_DISABLE_TIME * 9 / 10) { flightTime = 0; /* Just hit. Assume they'll get hit again */ } else { flightTime = MAX(0, flightTime - empTime); } } predict += Vector3i(iSinCosR(psDroid->sMove.moveDir, psDroid->sMove.speed * flightTime / GAME_TICKS_PER_SEC), 0); if (!isFlying(psDroid)) { predict.z = map_Height(predict.xy); // Predict that the object will be on the ground. } } /* Fire off the bullet to the miss location. The miss is only visible if the player owns the target. (Why? - Per) */ // What bVisible really does is to make the projectile audible even if it misses you. Since the target is NULL, proj_SendProjectile can't check if it was fired at you. bool bVisibleAnyway = psTarget->player == selectedPlayer; // see if we were lucky to hit the target bool isHit = gameRand(100) <= resultHitChance; if (isHit) { /* Kerrrbaaang !!!!! a hit */ objTrace(psAttacker->id, "combFire: [%s]->%u: resultHitChance=%d, visibility=%d", getName(psStats), psTarget->id, resultHitChance, (int)psTarget->visible[psAttacker->player]); syncDebug("hit=(%d,%d,%d)", predict.x, predict.y, predict.z); } else /* Deal with a missed shot */ { const int minOffset = 5; int missDist = 2 * (100 - resultHitChance) + minOffset; Vector3i miss = Vector3i(iSinCosR(gameRand(DEG(360)), missDist), 0); predict += miss; psTarget = nullptr; // Missed the target, so don't expect to hit it. objTrace(psAttacker->id, "combFire: Missed shot by (%4d,%4d)", miss.x, miss.y); syncDebug("miss=(%d,%d,%d)", predict.x, predict.y, predict.z); } // Make sure we don't pass any negative or out of bounds numbers to proj_SendProjectile CLIP(predict.x, 0, world_coord(mapWidth - 1)); CLIP(predict.y, 0, world_coord(mapHeight - 1)); proj_SendProjectileAngled(psWeap, psAttacker, psAttacker->player, predict, psTarget, bVisibleAnyway, weapon_slot, min_angle, fireTime); return true; }
/* Calculate the acceleration that the camera spins around at */ static void updateCameraRotationAcceleration(UBYTE update) { SDWORD worldAngle; float separation; SDWORD xConcern, yConcern, zConcern; bool bTooLow; PROPULSION_STATS *psPropStats; bool bGotFlying = false; SDWORD xPos = 0, yPos = 0, zPos = 0; bTooLow = false; if (trackingCamera.target->type == OBJ_DROID) { DROID *psDroid = (DROID *)trackingCamera.target; psPropStats = asPropulsionStats + psDroid->asBits[COMP_PROPULSION].nStat; if (psPropStats->propulsionType == PROPULSION_TYPE_LIFT) { int droidHeight, difHeight, droidMapHeight; bGotFlying = true; droidHeight = psDroid->pos.z; droidMapHeight = map_Height(psDroid->pos.x, psDroid->pos.y); difHeight = abs(droidHeight - droidMapHeight); if (difHeight < MIN_TRACK_HEIGHT) { bTooLow = true; } } } if (update & Y_UPDATE) { /* Presently only y rotation being calculated - but same idea for other axes */ /* Check what we're tracking */ if (getNumDroidsSelected() > 2 && trackingCamera.target->type == OBJ_DROID) { unsigned group = trackingCamera.target->selected ? GROUP_SELECTED : trackingCamera.target->group; yConcern = getAverageTrackAngle(group, false); } else { yConcern = trackingCamera.target->rot.direction; } yConcern += DEG(180); while (trackingCamera.rotation.y < 0) { trackingCamera.rotation.y += DEG(360); } /* Which way are we facing? */ worldAngle = trackingCamera.rotation.y; separation = angleDelta(yConcern - worldAngle); /* Make new acceleration */ trackingCamera.rotAccel.y = ROT_ACCEL_CONSTANT * separation - ROT_VELOCITY_CONSTANT * trackingCamera.rotVel.y; } if (update & X_UPDATE) { if (trackingCamera.target->type == OBJ_DROID && !bGotFlying) { uint16_t pitch; unsigned group = trackingCamera.target->selected ? GROUP_SELECTED : trackingCamera.target->group; getTrackingConcerns(&xPos, &yPos, &zPos, GROUP_SELECTED, true); // FIXME Should this be group instead of GROUP_SELECTED? getBestPitchToEdgeOfGrid(xPos, zPos, DEG(180) - getAverageTrackAngle(group, true), &pitch); pitch = MAX(angleDelta(pitch), DEG(14)); xConcern = -pitch; } else { xConcern = trackingCamera.target->rot.pitch; xConcern += DEG(-16); } while (trackingCamera.rotation.x < 0) { trackingCamera.rotation.x += DEG(360); } worldAngle = trackingCamera.rotation.x; separation = angleDelta(xConcern - worldAngle); /* Make new acceleration */ trackingCamera.rotAccel.x = /* Make this really slow */ ((ROT_ACCEL_CONSTANT) * separation - ROT_VELOCITY_CONSTANT * (float)trackingCamera.rotVel.x); } /* This looks a bit arse - looks like a flight sim */ if (update & Z_UPDATE) { if (bTooLow) { zConcern = 0; } else { zConcern = trackingCamera.target->rot.roll; } while (trackingCamera.rotation.z < 0) { trackingCamera.rotation.z += DEG(360); } worldAngle = trackingCamera.rotation.z; separation = (float)((zConcern - worldAngle)); if (separation < DEG(-180)) { separation += DEG(360); } else if (separation > DEG(180)) { separation -= DEG(360); } /* Make new acceleration */ trackingCamera.rotAccel.z = /* Make this really slow */ ((ROT_ACCEL_CONSTANT / 1) * separation - ROT_VELOCITY_CONSTANT * (float)trackingCamera.rotVel.z); } }