C++ (Cpp) tasks_run_imu_updateの例

コード例 #1

ファイル: imu.c プロジェクト: SyrianSpock/tp-mobile-robots

void imu_calibrate_gyros(imu_t *imu)
{
	int32_t i,j;
	tasks_run_imu_update(0);
	
	for (j = 0; j < 3; j++)
	{
		imu->calib_gyro.bias[j] = imu->oriented_gyro.data[j];
	}
	
	for (i = 0; i < 100; i++)
	{
		tasks_run_imu_update(0);

		//imu->imu->calib_sensor.bias[0 + ACC_OFFSET] = (0.9f * imu->imu->calib_accelero.bias[0] + 0.1f * (float)imu->oriented_accelero.data[0]);
		//imu->imu->calib_sensor.bias[1 + ACC_OFFSET] = (0.9f * imu->imu->calib_accelero.bias[1] + 0.1f * (float)imu->oriented_accelero.data[1]);
		//imu->imu->calib_sensor.bias[2 + ACC_OFFSET] = (0.9f * imu->imu->calib_accelero.bias[2] + 0.1f * ((float)imu->oriented_accelero.data[2] - imu->calib_accelero.scale_factor[2]));
		for (j = 0; j < 3; j++)
		{
			imu->calib_gyro.bias[j] = 0.9f * imu->calib_gyro.bias[j] + 0.1f * imu->oriented_gyro.data[j];
			//imu->attitude.raw_mag_mean[j] = (1.0 - fMAG_LPF) * imu->attitude.raw_mag_mean[j] + MAG_LPF * ((float)imu->oriented_compass.data[j]);
		}
	
		delay_ms(4);
	}
}

コード例 #2

ファイル: 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;
}

コード例 #3

ファイル: tasks.c プロジェクト: SyrianSpock/tp-mobile-robots

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

	if( mode.ARMED == ARMED_OFF )
	{
		// Set command to current heading
		central_data->command.attitude.rpy[2] = coord_conventions_quat_to_aero(central_data->ahrs.qe).rpy[2];
		servos_set_value_failsafe( &central_data->servos );
	}
	else if( mode.AUTO == AUTO_ON )
	{
		vector_field_waypoint_update( &central_data->vector_field_waypoint );
		velocity_controller_copter_update( &central_data->velocity_controller );
		attitude_controller_update( &central_data->attitude_controller );
		servos_mix_quadcopter_diag_update( &central_data->servo_mix );
	}
	else if( mode.MANUAL == MANUAL_ON && mode.GUIDED == GUIDED_ON )
	{
		remote_get_velocity_vector_from_remote(&central_data->remote, &central_data->controls);

		central_data->command.velocity.xyz[X] = central_data->controls.tvel[X];
		central_data->command.velocity.xyz[Y] = central_data->controls.tvel[Y];
		central_data->command.velocity.xyz[Z] = central_data->controls.tvel[Z];
		central_data->command.velocity.mode   = VELOCITY_COMMAND_MODE_GLOBAL;

		velocity_controller_copter_update( &central_data->velocity_controller );
		attitude_controller_update( &central_data->attitude_controller );
		servos_mix_quadcopter_diag_update( &central_data->servo_mix );
	}
	else if( mode.MANUAL == MANUAL_ON && mode.STABILISE == STABILISE_ON )
	{	
		remote_get_command_from_remote(&central_data->remote, &central_data->controls);
		
		central_data->command.attitude.rpy[0] 	= central_data->controls.rpy[0];
		central_data->command.attitude.rpy[1] 	= central_data->controls.rpy[1];
		central_data->command.attitude.rpy[2] 	+= 0.02 * central_data->controls.rpy[2];
		central_data->command.attitude.mode 	= ATTITUDE_COMMAND_MODE_RPY;
		central_data->command.thrust.thrust 	= central_data->controls.thrust;
	
		attitude_controller_update( &central_data->attitude_controller );

		// altitude
		// if( mode.GUIDED == GUIDED_ON )
		// {
		// 	float delta_h = 1.0 - central_data->sonar_i2cxl.data.current_distance;
		// 	float k_h = 0.2f;
		// 	central_data->command.thrust.thrust += k_h * delta_h;
		// }
			
		servos_mix_quadcopter_diag_update( &central_data->servo_mix );
	}
	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.ARMED == ARMED_ON && mode.HIL == HIL_OFF )
	{
		pwm_servos_write_to_hardware( &central_data->servos );
	}
	
	return TASK_RUN_SUCCESS;
} 

コード例 #4

ファイル: 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;
}

コード例 #5

ファイル: tasks.cpp プロジェクト: froj/MAVRIC_Library

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

    const State& state = central_data->state;

    if (!state.is_armed())
    {
        // Set command to current heading
        central_data->command.attitude.rpy[2] = coord_conventions_quat_to_aero(central_data->ahrs.qe).rpy[2];
        central_data->servo_0.failsafe();
        central_data->servo_1.failsafe();
        central_data->servo_2.failsafe();
        central_data->servo_3.failsafe();
    }
    else if (state.is_auto())
    {
        // vector_field_waypoint_update(&central_data->vector_field_waypoint);
        // velocity_controller_copter_update(&central_data->velocity_controller);
        // attitude_controller_update(&central_data->attitude_controller);
        // servos_mix_quadcopter_diag_update(&central_data->servo_mix);
    }
    else if (state.is_manual() && state.is_guided())
    {
        // manual_control_get_velocity_command(&central_data->manual_control, &central_data->command.velocity, 1.0f);
        // velocity_controller_copter_update(&central_data->velocity_controller);

        // get attitude command from remote
        central_data->manual_control.get_attitude_command(0.02f, &central_data->command.attitude, 1.0f);

        // Hardcode altitude command
        central_data->command.position.xyz[0] = 0.0f;
        central_data->command.position.xyz[1] = 0.0f;
        central_data->command.position.xyz[2] = -5.0f;
        central_data->command.position.mode   = POSITION_COMMAND_MODE_LOCAL;

        // Do control
        central_data->altitude_controller_.update();
        attitude_controller_update(&central_data->attitude_controller);

        servos_mix_quadcopter_diag_update(&central_data->servo_mix);
    }
    else if (state.is_manual() && state.is_stabilize())
    {
        // get command from remote
        central_data->manual_control.get_attitude_command(0.02f, &central_data->command.attitude, 1.0f);
        central_data->manual_control.get_thrust_command(&central_data->command.thrust);

        // Do control
        attitude_controller_update(&central_data->attitude_controller);

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

    return true;
}