C++ (Cpp) stabilisation_copter_cascade_stabilise示例

示例#1

文件： tasks.c 项目： SyrianSpock/tp-mobile-robots

task_return_t tasks_run_stabilisation(void* arg) 
{
	tasks_run_imu_update(0);
	
	mav_mode_t mode = central_data->state.mav_mode;

	if( mode.ARMED == ARMED_ON )
	{
		if ( mode.AUTO == AUTO_ON )
		{
			central_data->controls = central_data->controls_nav;
			central_data->controls.control_mode = VELOCITY_COMMAND_MODE;

			// if no waypoints are set, we do position hold therefore the yaw mode is absolute
			if (((central_data->state.nav_plan_active&&(!central_data->navigation.auto_takeoff)&&(!central_data->navigation.auto_landing)))||((central_data->state.mav_state == MAV_STATE_CRITICAL)&&(central_data->navigation.critical_behavior == FLY_TO_HOME_WP)))
			{
				central_data->controls.yaw_mode = YAW_COORDINATED;
			}
			else
			{
				central_data->controls.yaw_mode = YAW_ABSOLUTE;
			}

			if (central_data->state.in_the_air || central_data->navigation.auto_takeoff)
			{
				stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
			}
		}
		else if ( mode.GUIDED == GUIDED_ON )
		{
			central_data->controls = central_data->controls_nav;
			central_data->controls.control_mode = VELOCITY_COMMAND_MODE;

			if ((central_data->state.mav_state == MAV_STATE_CRITICAL) && (central_data->navigation.critical_behavior == FLY_TO_HOME_WP))
			{
				central_data->controls.yaw_mode = YAW_COORDINATED;
			}
			else
			{
				central_data->controls.yaw_mode = YAW_ABSOLUTE;
			}

			if (central_data->state.in_the_air || central_data->navigation.auto_takeoff)
			{
				stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
			}
		}
		else if ( mode.STABILISE == STABILISE_ON )
		{
			if (central_data->state.remote_active == 1)
			{
				remote_get_velocity_vector_from_remote(&central_data->remote, &central_data->controls);
			}
			else
			{
				joystick_parsing_get_velocity_vector_from_joystick(&central_data->joystick_parsing,&central_data->controls);
			}
			
			
			central_data->controls.control_mode = VELOCITY_COMMAND_MODE;
			central_data->controls.yaw_mode = YAW_RELATIVE;
			
			if (central_data->state.in_the_air || central_data->navigation.auto_takeoff)
			{
				stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
			}		
		}
		else if ( mode.MANUAL == MANUAL_ON )
		{
			if (central_data->state.remote_active == 1)
			{
				remote_get_command_from_remote(&central_data->remote, &central_data->controls);
			}
			else
			{
				joystick_parsing_get_attitude_command_from_joystick(&central_data->joystick_parsing,&central_data->controls);
			}
			
			central_data->controls.control_mode = ATTITUDE_COMMAND_MODE;
			central_data->controls.yaw_mode=YAW_RELATIVE;
		
			stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);		
		}
		else
		{
			servos_set_value_failsafe( &central_data->servos );
		}
	}
	else
	{
		servos_set_value_failsafe( &central_data->servos );
	}

		
	// !!! -- for safety, this should remain the only place where values are written to the servo outputs! --- !!!
	if ( mode.HIL == HIL_OFF )
	{
		pwm_servos_write_to_hardware( &central_data->servos );
	}
	
	return TASK_RUN_SUCCESS;
}

示例#2

文件： tasks.cpp 项目： froj/MAVRIC_Library

bool tasks_run_stabilisation(Central_data* central_data)
{
    tasks_run_imu_update(central_data);

    const State& state = central_data->state;

    if (state.is_armed())
    {
        if (state.is_auto())
        {
            central_data->controls = central_data->controls_nav;
            central_data->controls.control_mode = VELOCITY_COMMAND_MODE;

            // if no waypoints are set, we do position hold therefore the yaw mode is absolute
            if (((central_data->state.nav_plan_active && (central_data->navigation.internal_state_ == Navigation::NAV_NAVIGATING)) || (central_data->navigation.internal_state_ == Navigation::NAV_STOP_THERE)) || ((central_data->state.mav_state_ == MAV_STATE_CRITICAL) && (central_data->navigation.critical_behavior == Navigation::FLY_TO_HOME_WP)))
            {
                central_data->controls.yaw_mode = YAW_RELATIVE;
            }
            else
            {
                central_data->controls.yaw_mode = YAW_ABSOLUTE;
            }

            //if (central_data->state.in_the_air || central_data->navigation.auto_takeoff)
            if (true)//central_data->navigation.internal_state_ > NAV_ON_GND)
            {
                stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
                servos_mix_quadcopter_diag_update(&central_data->servo_mix);
            }
        }
        else if (state.is_guided())
        {
            central_data->controls = central_data->controls_nav;
            central_data->controls.control_mode = VELOCITY_COMMAND_MODE;

            if ((central_data->state.mav_state_ == MAV_STATE_CRITICAL) && (central_data->navigation.critical_behavior == Navigation::FLY_TO_HOME_WP))
            {
                central_data->controls.yaw_mode = YAW_RELATIVE;
            }
            else
            {
                central_data->controls.yaw_mode = YAW_ABSOLUTE;
            }

            //if (central_data->state.in_the_air || central_data->navigation.auto_takeoff)
            if (true)//central_data->navigation.internal_state_ > NAV_ON_GND)
            {
                stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
                servos_mix_quadcopter_diag_update(&central_data->servo_mix);
            }
        }
        else if (state.is_stabilize())
        {
            central_data->manual_control.get_velocity_vector(&central_data->controls);

            central_data->controls.control_mode = VELOCITY_COMMAND_MODE;
            central_data->controls.yaw_mode = YAW_RELATIVE;

            //if (central_data->state.in_the_air || central_data->navigation.auto_takeoff)
            if (true)//central_data->navigation.internal_state_ > NAV_ON_GND)
            {
                stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
                servos_mix_quadcopter_diag_update(&central_data->servo_mix);
            }
        }
        else if (state.is_manual())
        {
            central_data->manual_control.get_control_command(&central_data->controls);

            central_data->controls.control_mode = ATTITUDE_COMMAND_MODE;
            central_data->controls.yaw_mode = YAW_RELATIVE;

            stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
            servos_mix_quadcopter_diag_update(&central_data->servo_mix);
        }
        else
        {
            central_data->servo_0.failsafe();
            central_data->servo_1.failsafe();
            central_data->servo_2.failsafe();
            central_data->servo_3.failsafe();
        }
    }
    else
    {
        central_data->servo_0.failsafe();
        central_data->servo_1.failsafe();
        central_data->servo_2.failsafe();
        central_data->servo_3.failsafe();
    }

    return true;
}