aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: renaming functions after moving xy control

Browse Source
This commit is contained in:
Randy Mackay 2014-01-22 11:42:16 +09:00 committed by Andrew Tridgell
parent b9ae3ce2ff
commit 0a2adbac1b
5 changed files with 25 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/ArduCopter.pde
View File

@ -1773,7 +1773,7 @@ void update_roll_pitch_mode(void)
update_simple_mode(); update_simple_mode();
// update loiter target from user controls // update loiter target from user controls
wp_nav.move_loiter_target(g.rc_1.control_in, g.rc_2.control_in, 0.01f); wp_nav.set_pilot_desired_acceleration(g.rc_1.control_in, g.rc_2.control_in);
// copy latest output from nav controller to stabilize controller // copy latest output from nav controller to stabilize controller
control_roll = wp_nav.get_roll(); control_roll = wp_nav.get_roll();

7
ArduCopter/Log.pde
View File

@ -280,9 +280,9 @@ struct PACKED log_Nav_Tuning {
// Write an Nav Tuning packet // Write an Nav Tuning packet
static void Log_Write_Nav_Tuning() static void Log_Write_Nav_Tuning()
{ {
const Vector3f &pos_target = pos_controller.get_pos_target(); const Vector3f &pos_target = pos_control.get_pos_target();
const Vector3f &vel_target = pos_controller.get_vel_target(); const Vector3f &vel_target = pos_control.get_vel_target();
const Vector3f &accel_target = pos_controller.get_accel_target(); const Vector3f &accel_target = pos_control.get_accel_target();
const Vector3f &position = inertial_nav.get_position(); const Vector3f &position = inertial_nav.get_position();
const Vector3f &velocity = inertial_nav.get_velocity(); const Vector3f &velocity = inertial_nav.get_velocity();
@ -301,6 +301,7 @@ static void Log_Write_Nav_Tuning()
desired_accel_y : accel_target.y desired_accel_y : accel_target.y
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
cliSerial->printf_P(PSTR("\nX:%4.2f Y:%4.2f\n"),(float)pos_target.x,(float)pos_target.y);
} }
struct PACKED log_Control_Tuning { struct PACKED log_Control_Tuning {

16
ArduCopter/commands_logic.pde
View File

@ -315,10 +315,10 @@ static void do_land(const struct Location *cmd)
// calculate and set desired location above landing target // calculate and set desired location above landing target
Vector3f pos = pv_location_to_vector(*cmd); Vector3f pos = pv_location_to_vector(*cmd);
pos.z = min(current_loc.alt, RTL_ALT_MAX); pos.z = min(current_loc.alt, RTL_ALT_MAX);
wp_nav.set_destination(pos); wp_nav.set_wp_destination(pos);
// initialise original_wp_bearing which is used to check if we have missed the waypoint // initialise original_wp_bearing which is used to check if we have missed the waypoint
wp_bearing = wp_nav.get_bearing_to_destination(); wp_bearing = wp_nav.get_wp_bearing_to_destination();
original_wp_bearing = wp_bearing; original_wp_bearing = wp_bearing;
}else{ }else{
// set landing state // set landing state
@ -368,7 +368,7 @@ static void do_loiter_unlimited()
// use current location if not provided // use current location if not provided
if(command_nav_queue.lat == 0 && command_nav_queue.lng == 0) { if(command_nav_queue.lat == 0 && command_nav_queue.lng == 0) {
wp_nav.get_wp_stopping_point(target_pos); wp_nav.get_wp_stopping_point_xy(target_pos);
}else{ }else{
// default to use position provided // default to use position provided
target_pos = pv_location_to_vector(command_nav_queue); target_pos = pv_location_to_vector(command_nav_queue);
@ -436,7 +436,7 @@ static void do_loiter_time()
// use current location if not provided // use current location if not provided
if(command_nav_queue.lat == 0 && command_nav_queue.lng == 0) { if(command_nav_queue.lat == 0 && command_nav_queue.lng == 0) {
wp_nav.get_wp_stopping_point(target_pos); wp_nav.get_wp_stopping_point_xy(target_pos);
}else{ }else{
// default to use position provided // default to use position provided
target_pos = pv_location_to_vector(command_nav_queue); target_pos = pv_location_to_vector(command_nav_queue);
@ -478,7 +478,7 @@ static bool verify_land()
// check if we've reached the location // check if we've reached the location
if (wp_nav.reached_wp_destination()) { if (wp_nav.reached_wp_destination()) {
// get destination so we can use it for loiter target // get destination so we can use it for loiter target
Vector3f dest = wp_nav.get_destination(); Vector3f dest = wp_nav.get_wp_destination();
// switch into loiter nav mode // switch into loiter nav mode
set_nav_mode(NAV_LOITER); set_nav_mode(NAV_LOITER);
@ -591,7 +591,7 @@ static bool verify_RTL()
// use projection of safe stopping point based on current location and velocity // use projection of safe stopping point based on current location and velocity
Vector3f origin, dest; Vector3f origin, dest;
pos_control.get_stopping_point(origin); wp_nav.get_wp_stopping_point_xy(origin);
dest.x = origin.x; dest.x = origin.x;
dest.y = origin.y; dest.y = origin.y;
dest.z = get_RTL_alt(); dest.z = get_RTL_alt();
@ -607,7 +607,7 @@ static bool verify_RTL()
wp_nav.set_wp_destination(Vector3f(0,0,get_RTL_alt())); wp_nav.set_wp_destination(Vector3f(0,0,get_RTL_alt()));
// initialise original_wp_bearing which is used to point the nose home // initialise original_wp_bearing which is used to point the nose home
wp_bearing = wp_nav.get_bearing_to_destination(); wp_bearing = wp_nav.get_wp_bearing_to_destination();
original_wp_bearing = wp_bearing; original_wp_bearing = wp_bearing;
// advance to next rtl state // advance to next rtl state
@ -624,7 +624,7 @@ static bool verify_RTL()
wp_nav.set_wp_destination(Vector3f(0,0,get_RTL_alt())); wp_nav.set_wp_destination(Vector3f(0,0,get_RTL_alt()));
// initialise original_wp_bearing which is used to point the nose home // initialise original_wp_bearing which is used to point the nose home
wp_bearing = wp_nav.get_bearing_to_destination(); wp_bearing = wp_nav.get_wp_bearing_to_destination();
original_wp_bearing = wp_bearing; original_wp_bearing = wp_bearing;
// point nose towards home (maybe) // point nose towards home (maybe)

20
ArduCopter/events.pde
View File

@ -22,7 +22,7 @@ static void failsafe_radio_on_event()
}else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) { }else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
// if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately // if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
set_mode(LAND); set_mode(LAND);
}else if(home_distance > wp_nav.get_waypoint_radius()) { }else if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) { if (!set_mode(RTL)) {
set_mode(LAND); set_mode(LAND);
} }
@ -34,7 +34,7 @@ static void failsafe_radio_on_event()
case AUTO: case AUTO:
// failsafe_throttle is 1 do RTL, 2 means continue with the mission // failsafe_throttle is 1 do RTL, 2 means continue with the mission
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) { if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
if(home_distance > wp_nav.get_waypoint_radius()) { if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) { if (!set_mode(RTL)) {
set_mode(LAND); set_mode(LAND);
} }
@ -56,7 +56,7 @@ static void failsafe_radio_on_event()
}else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) { }else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
// if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately // if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
set_mode(LAND); set_mode(LAND);
}else if(home_distance > wp_nav.get_waypoint_radius()) { }else if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) { if (!set_mode(RTL)) {
set_mode(LAND); set_mode(LAND);
} }
@ -74,7 +74,7 @@ static void failsafe_radio_on_event()
if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) { if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
// if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately // if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
set_mode(LAND); set_mode(LAND);
}else if(home_distance > wp_nav.get_waypoint_radius()) { }else if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)){ if (!set_mode(RTL)){
set_mode(LAND); set_mode(LAND);
} }
@ -118,7 +118,7 @@ static void failsafe_battery_event(void)
init_disarm_motors(); init_disarm_motors();
}else{ }else{
// set mode to RTL or LAND // set mode to RTL or LAND
if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_waypoint_radius()) { if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) { if (!set_mode(RTL)) {
set_mode(LAND); set_mode(LAND);
} }
@ -129,7 +129,7 @@ static void failsafe_battery_event(void)
break; break;
case AUTO: case AUTO:
// set mode to RTL or LAND // set mode to RTL or LAND
if (home_distance > wp_nav.get_waypoint_radius()) { if (home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) { if (!set_mode(RTL)) {
set_mode(LAND); set_mode(LAND);
} }
@ -146,7 +146,7 @@ static void failsafe_battery_event(void)
} }
default: default:
// set mode to RTL or LAND // set mode to RTL or LAND
if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_waypoint_radius()) { if (g.failsafe_battery_enabled == FS_BATT_RTL && home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) { if (!set_mode(RTL)) {
set_mode(LAND); set_mode(LAND);
} }
@ -264,7 +264,7 @@ static void failsafe_gcs_check()
// if throttle is zero disarm motors // if throttle is zero disarm motors
if (g.rc_3.control_in == 0) { if (g.rc_3.control_in == 0) {
init_disarm_motors(); init_disarm_motors();
}else if(home_distance > wp_nav.get_waypoint_radius()) { }else if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) { if (!set_mode(RTL)) {
set_mode(LAND); set_mode(LAND);
} }
@ -276,7 +276,7 @@ static void failsafe_gcs_check()
case AUTO: case AUTO:
// if g.failsafe_gcs is 1 do RTL, 2 means continue with the mission // if g.failsafe_gcs is 1 do RTL, 2 means continue with the mission
if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL) { if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL) {
if (home_distance > wp_nav.get_waypoint_radius()) { if (home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) { if (!set_mode(RTL)) {
set_mode(LAND); set_mode(LAND);
} }
@ -288,7 +288,7 @@ static void failsafe_gcs_check()
// if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything // if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
break; break;
default: default:
if(home_distance > wp_nav.get_waypoint_radius()) { if(home_distance > wp_nav.get_wp_radius()) {
if (!set_mode(RTL)) { if (!set_mode(RTL)) {
set_mode(LAND); set_mode(LAND);
} }

4
ArduCopter/navigation.pde
View File

@ -92,7 +92,7 @@ static bool set_nav_mode(uint8_t new_nav_mode)
case NAV_CIRCLE: case NAV_CIRCLE:
// set center of circle to current position // set center of circle to current position
pos_control.get_stopping_point(stopping_point); wp_nav.get_loiter_stopping_point_xy(stopping_point);
circle_set_center(stopping_point,ahrs.yaw); circle_set_center(stopping_point,ahrs.yaw);
nav_initialised = true; nav_initialised = true;
break; break;
@ -217,7 +217,7 @@ circle_set_center(const Vector3f current_position, float heading_in_radians)
circle_angular_velocity_max = constrain_float(ToRad(g.circle_rate),-circle_angular_velocity_max,circle_angular_velocity_max); circle_angular_velocity_max = constrain_float(ToRad(g.circle_rate),-circle_angular_velocity_max,circle_angular_velocity_max);
// angular_velocity in radians per second // angular_velocity in radians per second
circle_angular_acceleration = wp_nav.get_waypoint_acceleration()/((float)g.circle_radius * 100); circle_angular_acceleration = wp_nav.get_wp_acceleration()/((float)g.circle_radius * 100);
if (g.circle_rate < 0.0f) { if (g.circle_rate < 0.0f) {
circle_angular_acceleration = -circle_angular_acceleration; circle_angular_acceleration = -circle_angular_acceleration;
} }