zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: renaming functions after moving xy control

master
Randy Mackay 11 years ago committed by Andrew Tridgell
parent
b9ae3ce2ff
commit
0a2adbac1b
5 changed files with 25 additions and 24 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      ArduCopter/ArduCopter.pde
  2. 7
      ArduCopter/Log.pde
  3. 16
      ArduCopter/commands_logic.pde
  4. 20
      ArduCopter/events.pde
  5. 4
      ArduCopter/navigation.pde

2
ArduCopter/ArduCopter.pde
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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;
} }

Write Preview
Loading…
Cancel
Save