Servers by jDrones

Doubts about adding a new flight mode that relies on AC_PosControl library

(Ahmed Osama Mohamed Elhassan Mahgoub) #1

Hello ArduPilot community!

I am trying to add a new mode to Copter, and this mode would be simply using IMU data to navigate (inertial navigation). I have used the mode guided as a guideline, and keeping in mind that I will skipp all the functions that would use GPS data. I am using coordinates in CM and using AC_PosControl library. I included the code below (this is not the run() function, but a rather a helper function that would be used by run(), because I have many targets).

However I have some doubts…
I am using the xy controller, but I don’t know how the coordinate system is defined, I mean where is the origin? is it the starting point? (The target coordinates I entered assumes that the starting point is the origin). Secondly… does the system keep track of the old origin? Or every time the origin is reset to your current position?

Thank you!

void Copter::ModeNew::go_to(int payload_num)
{

// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
    make_safe_spool_down();
    return;
}

// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

current_pos=inertial_nav.get_position();

switch(payload_num)
{
	case 0:
		target=home;
		break;
		
	case 1:
		target=payload_1;
		break;
		
	case 2:
		target=payload_2;
		break;
		
	case 3:
		target=payload_3;
		break;
		
	case 4:
		target=payload_4;
		break;
		
	case 5:
		target=payload_5;
		break;			
}

// Initialize controller and set target
pos_control->init_xy_controller();
pos_control->set_xy_target(target.x,target.y);
pos_control->set_target_to_stopping_point_xy();

// run position controller
pos_control->update_xy_controller();

// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
    // roll & pitch from waypoint controller, yaw rate from pilot
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), 0.0f);
} else if (auto_yaw.mode() == AUTO_YAW_RATE) {
    // roll & pitch from waypoint controller, yaw rate from mavlink command or mission item
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.rate_cds());
} else {
    // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
    attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.yaw(), true);
}

while(true)
{
	if(abs(target.y-current_pos.y)<=1)
	{
		break;
	}
}

}