Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: combine nav_roll and control_roll

Browse Source 
Control_roll, control_pitch now always hold the desired lean angle
whether it be from the pilot or autopilot.
This commit is contained in:
Randy Mackay 2013-11-27 23:11:30 +09:00
parent d7c636b4aa
commit a744658b47
3 changed files with 13 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
ArduCopter/ArduCopter.pde
View File

@ -700,12 +700,6 @@ static struct Location command_cond_queue;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Navigation Roll/Pitch functions // Navigation Roll/Pitch functions
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// all angles are deg * 100 : target yaw angle
// The Commanded ROll from the autopilot.
static int32_t nav_roll;
// The Commanded pitch from the autopilot. negative Pitch means go forward.
static int32_t nav_pitch;
// The Commanded ROll from the autopilot based on optical flow sensor. // The Commanded ROll from the autopilot based on optical flow sensor.
static int32_t of_roll; static int32_t of_roll;
// The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch means go forward. // The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch means go forward.
@ -1687,14 +1681,10 @@ void update_roll_pitch_mode(void)
case ROLL_PITCH_AUTO: case ROLL_PITCH_AUTO:
// copy latest output from nav controller to stabilize controller // copy latest output from nav controller to stabilize controller
nav_roll = wp_nav.get_desired_roll(); control_roll = wp_nav.get_desired_roll();
nav_pitch = wp_nav.get_desired_pitch(); control_pitch = wp_nav.get_desired_pitch();
get_stabilize_roll(nav_roll); get_stabilize_roll(control_roll);
get_stabilize_pitch(nav_pitch); get_stabilize_pitch(control_pitch);
// user input, although ignored is put into control_roll and pitch for reporting purposes
control_roll = g.rc_1.control_in;
control_pitch = g.rc_2.control_in;
break; break;
case ROLL_PITCH_STABLE_OF: case ROLL_PITCH_STABLE_OF:
@ -1717,19 +1707,15 @@ void update_roll_pitch_mode(void)
// apply SIMPLE mode transform // apply SIMPLE mode transform
update_simple_mode(); update_simple_mode();
// copy user input for reporting purposes
control_roll = g.rc_1.control_in;
control_pitch = g.rc_2.control_in;
// update loiter target from user controls // update loiter target from user controls
wp_nav.move_loiter_target(control_roll, control_pitch,0.01f); wp_nav.move_loiter_target(g.rc_1.control_in, g.rc_2.control_in, 0.01f);
// copy latest output from nav controller to stabilize controller // copy latest output from nav controller to stabilize controller
nav_roll = wp_nav.get_desired_roll(); control_roll = wp_nav.get_desired_roll();
nav_pitch = wp_nav.get_desired_pitch(); control_pitch = wp_nav.get_desired_pitch();
get_stabilize_roll(nav_roll); get_stabilize_roll(control_roll);
get_stabilize_pitch(nav_pitch); get_stabilize_pitch(control_pitch);
break; break;
case ROLL_PITCH_SPORT: case ROLL_PITCH_SPORT:

6
ArduCopter/GCS_Mavlink.pde
View File

@ -256,9 +256,9 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
{ {
mavlink_msg_nav_controller_output_send( mavlink_msg_nav_controller_output_send(
chan, chan,
nav_roll / 1.0e2f, control_roll / 1.0e2f,
nav_pitch / 1.0e2f, control_pitch / 1.0e2f,
wp_bearing / 1.0e2f, control_yaw / 1.0e2f,
wp_bearing / 1.0e2f, wp_bearing / 1.0e2f,
wp_distance / 1.0e2f, wp_distance / 1.0e2f,
altitude_error / 1.0e2f, altitude_error / 1.0e2f,

4
ArduCopter/navigation.pde
View File

@ -86,7 +86,7 @@ static bool set_nav_mode(uint8_t new_nav_mode)
case NAV_NONE: case NAV_NONE:
nav_initialised = true; nav_initialised = true;
// initialise global navigation variables including wp_distance, nav_roll // initialise global navigation variables including wp_distance
reset_nav_params(); reset_nav_params();
break; break;
@ -175,8 +175,6 @@ static void reset_nav_params(void)
// Will be set by nav or loiter controllers // Will be set by nav or loiter controllers
lon_error = 0; lon_error = 0;
lat_error = 0; lat_error = 0;
nav_roll = 0;
nav_pitch = 0;
} }
// get_yaw_slew - reduces rate of change of yaw to a maximum // get_yaw_slew - reduces rate of change of yaw to a maximum