#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);
    if (prev_control_mode == ALT_HOLD) {
        last_roll_s = ahrs.roll_sensor;
        last_pitch_s = ahrs.pitch_sensor;
    } else {
        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::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();

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

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