//#######################################################################################//
class ap_maneuvre:public iautopilot{
public:
ap_maneuvre(VESSEL *vessel);
void start();
void init();
void stop();
void step(double simt,double simdt);
void info();
VESSEL *tgt_ves;
char tgt_nam[255];
int use_grp,mode,dvdir;
double dv,dt,level;
char mode_is[255],dir_is[255];
};
//#######################################################################################//
ap_maneuvre::ap_maneuvre(VESSEL *vessel):iautopilot(vessel)
{
//Define and initialize variables
sprintf(name,"Maneuvre\0");
add_var("engine" ,VT_INT ,&use_grp,VF_INP,1,15);
add_var("mode" ,VT_INT ,&mode ,VF_INP,0,1);
add_var("target" ,VT_CHAR,tgt_nam ,VF_INP,0,255);
add_var("orientation",VT_INT ,&dvdir ,VF_INP,0,3);
add_var("dv" ,VT_DBL ,&dv ,VF_INP,0,1e10);
add_var("dt" ,VT_DBL ,&dt ,VF_INP,0,1e10);
add_var("level" ,VT_DBL ,&level ,VF_INP,0,1);
add_var("mode_is" ,VT_CHAR,&mode_is,VF_OUT,0,255);
add_var("dir_is" ,VT_CHAR,&dir_is ,VF_OUT,0,255);
sprintf(tgt_nam,"\0");
use_grp=xTHGROUP_MAIN;
mode=0;
dv=100;
dt=10;
dvdir=0;
level=1;
max_time_accel=10;
use_grp=xTHGROUP_MAIN;
info();
}
//#######################################################################################//
void ap_maneuvre::info()
{
//Process info variables
switch(mode){
case 0:set_char_var("mode_is","Timed burn");break;
case 1:set_char_var("mode_is","Dv burn");break;
default:set_char_var("mode_is","WTF?");
}
switch(dvdir){
case 0:set_char_var("dir_is","Prograde");break;
case 1:set_char_var("dir_is","Retrograde");break;
case 2:set_char_var("dir_is","From target");break;
case 3:set_char_var("dir_is","Towards target");break;
default:set_char_var("dir_is","N/A");
}
}
//#######################################################################################//
void ap_maneuvre::start()
{
//On start
iautopilot::start();
is_started=true;
is_running=true;
init();
}
//#######################################################################################//
void ap_maneuvre::init()
{
//Before all runs
is_loaded=true;
stop_all_engines();
tgt_ves=vessel_by_name(tgt_nam);
if((dvdir==2)||(dvdir==3))if(!tgt_ves){fail_error("No such target vessel");return;}
}
//#######################################################################################//
void ap_maneuvre::stop()
{
//On finish or stop
iautopilot::stop();
stop_all_engines();
}
//#######################################################################################//
void ap_maneuvre::step(double simt,double simdt)
{
//Every step
if(!is_running)return;
if(!is_loaded)init();
VECTOR3 ang_vel,pyr,d,rv,rp;
int can_burn=0;
double acc,lv;
ang_vel=get_angular_vel_grp(use_grp);
sprintf(state_string,"Orienting)");
if(dvdir==0){
pyr=get_pitchyawroll(global_2_local(us,get_relative_vel(us,get_gravity_ref(us))+get_global_pos(us)),global_2_local(us,get_relative_pos(us,get_gravity_ref(us))+get_global_pos(us)));
if((absd(pyr.x-0)<1*RAD)&&(absd(pyr.y-0)<1*RAD)&&(absd(pyr.z-0)<1*RAD)){
if((absd(ang_vel.x)<0.05)&&(absd(ang_vel.y)<0.05)&&(absd(ang_vel.z)<0.05))can_burn=1;
}
att_guide(simdt,0,0,0,1);
att_control_exp(simdt,pyr.x,pyr.y,pyr.z,0,0,0,7,1);
}else if(dvdir==1){
pyr=get_pitchyawroll(-global_2_local(us,get_relative_vel(us,get_gravity_ref(us))+get_global_pos(us)),global_2_local(us,get_relative_pos(us,get_gravity_ref(us))+get_global_pos(us)));
if((absd(pyr.x-0)<1*RAD)&&(absd(pyr.y-0)<1*RAD)&&(absd(pyr.z-0)<1*RAD)){
if((absd(ang_vel.x)<0.05)&&(absd(ang_vel.y)<0.05)&&(absd(ang_vel.z)<0.05))can_burn=1;
}
att_guide(simdt,0,0,0,1);
att_control_exp(simdt,pyr.x,pyr.y,pyr.z,0,0,0,7,1);
}else if((dvdir==2)||(dvdir==3)){
rv=get_obj_relative_vel(get_ves_obj(us),get_ves_obj(tgt_ves));
rp=get_obj_relative_pos(get_ves_obj(us),get_ves_obj(tgt_ves));
d=global_2_local(us,rp+get_global_pos(us));
if(dvdir==3)d=-d;
pyr=get_pitchyawroll(d,tvec(0,0,0));
if((absd(pyr.x-0)<1*RAD)&&(absd(pyr.y-0)<1*RAD)&&(absd(pyr.z-0)<1*RAD)){
if((absd(ang_vel.x)<0.05)&&(absd(ang_vel.y)<0.05)&&(absd(ang_vel.z)<0.05))can_burn=1;
}
att_guide(simdt,0,0,0,1);
att_control_exp(simdt,pyr.x,pyr.y,0,0,0,0,7,1);
}
if(can_burn){
sprintf(state_string,"Burning");
if(mode==0){
dt=dt-simdt;
if(dt<=0){stop();return;}
set_thgrp_level(level,use_grp);
}else if(mode==1){
if(dv<=0){stop();return;}
acc=modv(get_group_thrust_vector(use_grp,true))/get_ves_mass(us);
if(level*acc*simdt>dv)lv=dv/simdt;else lv=level*acc;
dv=dv-lv*simdt;
set_thgrp_level(lv/acc,use_grp);
}
}else set_thgrp_level(0,use_grp);
}