#include "Sub.h"

// stabilize_init - initialise stabilize controller
bool Sub::stabilize_init()
{
    // set target altitude to zero for reporting
    pos_control.set_alt_target(0);
    
    last_roll_s = 0;
    last_pitch_s = 0;
    
    last_yaw_s = ahrs.yaw_sensor;
    //last_input_ms = AP_HAL::millis();
	last_input_ms_stable = AP_HAL::millis();

	pitch_input_inc=0;
	//yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
	prepare_state = Horizontal;
    return true;
}

// stabilize_run - runs the main stabilize controller
// should be called at 100hz or more
extern mavlink_rov_state_monitoring_t rov_message;

void Sub::handle_attitude_stablize(){
	
	uint32_t tnow = AP_HAL::millis();

	motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

	// get pilot desired lean angles
	float target_roll, target_pitch, target_yaw;

	// Check if set_attitude_target_no_gps is valid
	if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
		Quaternion(
			set_attitude_target_no_gps.packet.q
		).to_euler(
			target_roll,
			target_pitch,
			target_yaw
		);
		target_roll = 100 * degrees(target_roll);
		target_pitch = 100 * degrees(target_pitch);
		target_yaw = 100 * degrees(target_yaw);
		last_roll_s = target_roll;
		last_pitch_s = target_pitch;
		last_yaw_s = target_yaw;
		
		attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, target_yaw, true);
	} else {
		
		//int32_t yaw_input= (int32_t)((float)yaw_press*100);
		//int32_t roll_input	= 0;
		int32_t pitch_input   = 0;
		

		if (prepare_state == Horizontal)
	   {
		   pitch_input = 0;
		   target_roll = 0;
		   last_roll_s = 0;
		   target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
	   }else if(prepare_state == Vertical)
		{
		   pitch_input = 8000;
		   target_roll = ((float)channel_yaw->get_control_in()*0.5);
		   
		   target_yaw = 0;
	   }
	   else{
			pitch_input = 0;
		}
	   pitch_input_inc = constrain_int32(pitch_input,-9000,9000);
	   //-------------------------------------
	  // target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
	  if (abs(target_roll) > 300 ){
		attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, 0, 0);
		last_yaw_s = ahrs.yaw_sensor;
		last_roll_s = ahrs.roll_sensor;
		last_input_ms_stable = tnow;
		
		}else if (abs(target_yaw) > 300) {
			// if only yaw is being controlled, don't update pitch and roll
			attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, target_yaw);
			last_yaw_s = ahrs.yaw_sensor;
			
			last_input_ms_stable = tnow;
		} else if (tnow < last_input_ms_stable + 250) {
			// just brake for a few mooments so we don't bounce
			last_yaw_s = ahrs.yaw_sensor;
			
			attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, 0);
		} else {
			// Lock attitude
			attitude_control.input_euler_angle_roll_pitch_yaw(last_roll_s, (float)pitch_input_inc, last_yaw_s, true);
			//attitude_control.input_euler_angle_roll_pitch_yaw(roll_input, (float)pitch_input_inc, last_yaw, true);
		}//--------------------------------------------------------------------------------------------------------------
	   //attitude_control.input_euler_angle_roll_pitch_yaw(roll_input, (float)pitch_input_inc, yaw_input, true);//pitch九十度 竖直前进有可能翻车
		//attitude_control.input_euler_angle_roll_pitch_yaw_quat_control((float)roll_input, (float)pitch_input_inc, (float)yaw_input, true);//四元数控制在pitch90度时的效果与欧拉角控制在80度时相差不多, 但是在360往0的YAW切换的时候会猛烈转动
		static int f = 0;
		f++;
		if(f>800)
		{
		//float cosruslt = sinf(radians(286)*0.5f);

			//gcs().send_text(MAV_SEVERITY_INFO, " stableinput %f %f,%f ",(float)last_roll_s,(float)pitch_input_inc,(float)last_yaw_s);
			f=0;
		}
		
	}
}

void Sub::stabilize_run()
{
    // if not armed set throttle to zero and exit immediately
    if (!motors.armed()) {
        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
        attitude_control.set_throttle_out(0,true,g.throttle_filt);
        attitude_control.relax_attitude_controllers();
        last_roll_s = 0;
        last_pitch_s = 0;
        last_yaw_s = ahrs.yaw_sensor;

		PressLevel_f =5.0;//压力分级复位
		PressLevel = no;
		//yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位
		pitch_input_inc=0;
		prepare_state = Horizontal;
        return;
    }
    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

    handle_attitude_stablize();

    motors.set_forward(channel_forward->norm_input());
    motors.set_lateral(channel_lateral->norm_input());

	attitude_control.set_throttle_out(1.0-(float)PressLevel_f*0.1, false, g.throttle_filt);//压力分级
	rov_message.pressure_level = int(PressLevel);
}