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