int command_turn(int delta_angle) { if (navdata_valid){ float32_t heading = navdata_unpacked.navdata_demo.psi; if (heading < 0) heading += 360000; heading += delta_angle; if (heading < 0) heading += 360000; if (heading > 360000) heading -= 360000; pilot_heading=heading; pilot_head=1; } else { return -1; } set_command_timestamp(); return 0; }
int command_cali(void) { addATCALIB(cmdbuffer); set_command_timestamp(); return 0; }
int command_head(int angle) { pilot_heading=angle; pilot_head=1; set_command_timestamp(); return 0; }
int command_alti(int altitude, int monitor_state) { pilot_altitude=altitude; pilot_alti=1; monitor_alti = monitor_state; set_command_timestamp(); return 0; }
int command_alti(int altitude, int monitor_state) { pilot_altitude=altitude; pilot_alti = 0; // Originally set to 1. By setting to 0, // we abandon height controllers in pilot.c // and in monitor.c, we use P-controller instead. monitor_alti = monitor_state; set_command_timestamp(); return 0; }
int command_rise(int delta_altitude) { if (navdata_valid){ /* Dont add height error here, we only increase by X */ int altitude = navdata_unpacked.navdata_demo.altitude; altitude += delta_altitude; pilot_altitude=altitude; pilot_alti=1; } else { return -1; } set_command_timestamp(); return 0; }
int command_move(int roll, int pitch, int gaz, int yaw) { drone_roll = roll; drone_pitch = pitch; drone_gaz = gaz; if (gaz != 0) { // cancel a running autopilot pilot_alti = 0; monitor_alti = 0; } drone_yaw = yaw; if (yaw != 0) { // cancel a running autopilot pilot_head = 0; } drone_pcmd_flags = DRONE_PCMD_FLAG_PROGRESSIVE; set_command_timestamp(); return 0; }
int command_state(int state) { if (state == 0) { pilot_alti = 0; pilot_head = 0; drone_fly = 0; } if (state == 1) drone_fly = 1; if (state == 666){ // Send an emergency message block seq=1; addATREF(cmdbuffer, DRONE_REF_FLAG_BASIC); addATREF(cmdbuffer, DRONE_REF_FLAG_BASIC | DRONE_REF_FLAG_EMERGENCY); addATREF(cmdbuffer, DRONE_REF_FLAG_BASIC); pilot_alti = 0; pilot_head = 0; drone_fly = 0; } if (state == 777){ // Send a recovery message block addATREF(cmdbuffer, DRONE_REF_FLAG_BASIC | DRONE_REF_FLAG_EMERGENCY); pilot_alti = 0; pilot_head = 0; drone_fly = 0; } set_command_timestamp(); return 0; }
int auto_pilot() { if (pilot_head) { // Continously turn until head is looking into // right direction if (navdata_valid){ float32_t heading = navdata_unpacked.navdata_demo.psi; if (heading < 0) heading += 360000; float32_t diff_angle = pilot_heading - heading; if (diff_angle < 0) diff_angle += 360000; // printf("diff: %f\n",diff_angle); if (diff_angle < 2000){ drone_yaw = 0; pilot_head = 0; return 1; } else { int speed = 150; if (abs(diff_angle > 10000)) speed = 300; if (abs(diff_angle > 20000)) speed = 600; if (abs(diff_angle > 35000)) speed = 900; if (diff_angle > 180000){ drone_yaw = -1 * speed; } else { drone_yaw = speed; } set_command_timestamp(); } } } if (pilot_alti) { // Continously rise or sink until at the // right altitude int alt = navdata_unpacked.navdata_demo.altitude + err_drone_altitude; int alt_diff=abs(alt-pilot_altitude); // printf("diff: %f\n",alt_diff); if (alt_diff < 200){ drone_gaz = 0; pilot_alti = 0; return 1; } else { int speed = 400; if (alt_diff > 800) speed = 900; if(alt < pilot_altitude){ drone_gaz = speed; } else { drone_gaz = -1 * speed; } set_command_timestamp(); } } return 0; }