2011-03-19 07:20:11 -03:00
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2010-12-19 12:40:33 -04:00
2013-02-18 01:58:24 -04:00
// run_nav_updates - top level call for the autopilot
// ensures calculations such as "distance to waypoint" are calculated before autopilot makes decisions
// To-Do - rename and move this function to make it's purpose more clear
2013-01-08 01:45:12 -04:00
static void run_nav_updates(void)
{
2013-02-18 01:58:24 -04:00
// fetch position from inertial navigation
calc_position();
2013-01-08 01:45:12 -04:00
2013-02-18 01:58:24 -04:00
// calculate distance and bearing for reporting and autopilot decisions
calc_distance_and_bearing();
2013-01-25 02:11:09 -04:00
2013-02-18 01:58:24 -04:00
// run autopilot to make high level decisions about control modes
run_autopilot();
}
// calc_position - get lat and lon positions from inertial nav library
static void calc_position(){
if( inertial_nav.position_ok() ) {
2013-01-25 02:11:09 -04:00
// pull position from interial nav library
current_loc.lng = inertial_nav.get_longitude();
current_loc.lat = inertial_nav.get_latitude();
}
2012-11-07 06:03:30 -04:00
}
2013-02-18 01:58:24 -04:00
// calc_distance_and_bearing - calculate distance and direction to waypoints for reporting and autopilot decisions
2012-11-07 06:03:30 -04:00
static void calc_distance_and_bearing()
{
2013-03-20 10:29:08 -03:00
Vector3f curr = inertial_nav.get_position();
2013-10-29 01:28:27 -03:00
2013-02-18 01:58:24 -04:00
// get target from loiter or wpinav controller
if( nav_mode == NAV_LOITER || nav_mode == NAV_CIRCLE ) {
2013-03-20 10:29:08 -03:00
wp_distance = wp_nav.get_distance_to_target();
wp_bearing = wp_nav.get_bearing_to_target();
2013-02-18 01:58:24 -04:00
}else if( nav_mode == NAV_WP ) {
2013-03-20 10:29:08 -03:00
wp_distance = wp_nav.get_distance_to_destination();
wp_bearing = wp_nav.get_bearing_to_destination();
2013-01-28 02:59:55 -04:00
}else{
2013-03-20 10:29:08 -03:00
wp_distance = 0;
2013-01-28 02:59:55 -04:00
wp_bearing = 0;
}
2012-11-07 06:03:30 -04:00
2013-01-28 02:59:55 -04:00
// calculate home distance and bearing
2013-11-11 09:24:18 -04:00
if(GPS_ok()) {
2013-03-20 10:29:08 -03:00
home_distance = pythagorous2(curr.x, curr.y);
home_bearing = pv_get_bearing_cd(curr,Vector3f(0,0,0));
2012-12-08 01:23:32 -04:00
2013-01-28 02:59:55 -04:00
// update super simple bearing (if required) because it relies on home_bearing
2013-10-05 06:25:03 -03:00
update_super_simple_bearing(false);
2013-02-18 01:58:24 -04:00
}
2012-04-19 01:06:15 -03:00
}
2013-01-24 00:36:55 -04:00
// run_autopilot - highest level call to process mission commands
static void run_autopilot()
2012-11-07 06:03:30 -04:00
{
2013-01-24 00:36:55 -04:00
switch( control_mode ) {
case AUTO:
2013-04-24 08:59:49 -03:00
// load the next command if the command queues are empty
update_commands();
// process the active navigation and conditional commands
2013-01-24 00:36:55 -04:00
verify_commands();
break;
case GUIDED:
2013-03-21 06:29:38 -03:00
// no need to do anything - wp_nav should take care of getting us to the desired location
break;
2013-01-24 00:36:55 -04:00
case RTL:
verify_RTL();
break;
}
}
2012-11-07 06:03:30 -04:00
2013-01-24 00:36:55 -04:00
// set_nav_mode - update nav mode and initialise any variables as required
static bool set_nav_mode(uint8_t new_nav_mode)
{
2013-06-15 06:14:36 -03:00
bool nav_initialised = false; // boolean to ensure proper initialisation of nav modes
Vector3f stopping_point; // stopping point for circle mode
2012-11-07 06:03:30 -04:00
2013-01-24 00:36:55 -04:00
// return immediately if no change
if( new_nav_mode == nav_mode ) {
return true;
}
2012-11-07 06:03:30 -04:00
2013-01-24 00:36:55 -04:00
switch( new_nav_mode ) {
2012-11-07 06:03:30 -04:00
2013-01-24 00:36:55 -04:00
case NAV_NONE:
nav_initialised = true;
2013-11-27 10:11:30 -04:00
// initialise global navigation variables including wp_distance
2013-09-08 22:37:44 -03:00
reset_nav_params();
2013-01-24 00:36:55 -04:00
break;
2012-11-07 06:03:30 -04:00
2013-01-24 00:36:55 -04:00
case NAV_CIRCLE:
2013-02-25 04:50:56 -04:00
// set center of circle to current position
2013-06-15 06:14:36 -03:00
wp_nav.get_stopping_point(inertial_nav.get_position(),inertial_nav.get_velocity(),stopping_point);
circle_set_center(stopping_point,ahrs.yaw);
2013-01-24 00:36:55 -04:00
nav_initialised = true;
break;
2012-11-07 06:03:30 -04:00
2013-01-24 00:36:55 -04:00
case NAV_LOITER:
// set target to current position
2013-06-15 06:14:36 -03:00
wp_nav.init_loiter_target(inertial_nav.get_position(), inertial_nav.get_velocity());
2013-01-24 00:36:55 -04:00
nav_initialised = true;
break;
2012-11-07 06:03:30 -04:00
2013-01-24 00:36:55 -04:00
case NAV_WP:
nav_initialised = true;
break;
2012-11-07 06:03:30 -04:00
}
2013-01-24 00:36:55 -04:00
// if initialisation has been successful update the yaw mode
if( nav_initialised ) {
nav_mode = new_nav_mode;
2012-11-07 06:03:30 -04:00
}
2013-01-24 00:36:55 -04:00
// return success or failure
return nav_initialised;
2012-11-07 06:03:30 -04:00
}
2013-01-24 00:36:55 -04:00
// update_nav_mode - run navigation controller based on nav_mode
2013-08-27 23:33:10 -03:00
// called at 100hz
2013-01-24 00:36:55 -04:00
static void update_nav_mode()
2012-12-09 02:24:19 -04:00
{
2013-08-27 23:33:10 -03:00
static uint8_t log_counter; // used to slow NTUN logging
// exit immediately if not auto_armed or inertial nav position bad
if (!ap.auto_armed || !inertial_nav.position_ok()) {
return;
}
2013-01-24 00:36:55 -04:00
switch( nav_mode ) {
2012-12-09 02:24:19 -04:00
2013-01-24 00:36:55 -04:00
case NAV_NONE:
// do nothing
2012-12-09 02:24:19 -04:00
break;
2013-01-24 00:36:55 -04:00
case NAV_CIRCLE:
2013-02-25 04:50:56 -04:00
// call circle controller which in turn calls loiter controller
2013-08-27 23:33:10 -03:00
update_circle();
2013-01-24 00:36:55 -04:00
break;
2012-12-09 02:24:19 -04:00
2013-01-24 00:36:55 -04:00
case NAV_LOITER:
2013-07-26 02:15:27 -03:00
// reset target if we are still on the ground
if (ap.land_complete) {
wp_nav.init_loiter_target(inertial_nav.get_position(),inertial_nav.get_velocity());
2013-07-26 09:26:28 -03:00
}else{
// call loiter controller
wp_nav.update_loiter();
2013-07-26 02:15:27 -03:00
}
2013-01-25 02:11:09 -04:00
break;
2013-01-27 10:48:07 -04:00
2013-02-18 01:58:24 -04:00
case NAV_WP:
2013-03-20 10:29:08 -03:00
// call waypoint controller
wp_nav.update_wpnav();
2013-01-27 10:48:07 -04:00
break;
2013-01-24 00:36:55 -04:00
}
2012-12-09 02:24:19 -04:00
2013-08-27 23:33:10 -03:00
// log to dataflash at 10hz
log_counter++;
if (log_counter >= 10 && (g.log_bitmask & MASK_LOG_NTUN) && nav_mode != NAV_NONE) {
log_counter = 0;
2013-05-19 10:53:35 -03:00
Log_Write_Nav_Tuning();
}
2012-12-09 02:24:19 -04:00
}
2012-11-23 02:57:49 -04:00
// Keeps old data out of our calculation / logs
static void reset_nav_params(void)
{
// Will be set by new command
2012-12-08 01:23:32 -04:00
wp_bearing = 0;
2012-11-23 02:57:49 -04:00
// Will be set by new command
wp_distance = 0;
2013-02-18 01:58:24 -04:00
// Will be set by nav or loiter controllers
lon_error = 0;
2012-11-23 02:57:49 -04:00
lat_error = 0;
}
2012-01-11 03:38:16 -04:00
2012-12-08 01:23:32 -04:00
// get_yaw_slew - reduces rate of change of yaw to a maximum
// assumes it is called at 100hz so centi-degrees and update rate cancel each other out
static int32_t get_yaw_slew(int32_t current_yaw, int32_t desired_yaw, int16_t deg_per_sec)
{
2013-04-21 10:44:57 -03:00
return wrap_360_cd(current_yaw + constrain_int16(wrap_180_cd(desired_yaw - current_yaw), -deg_per_sec, deg_per_sec));
2012-12-14 00:12:39 -04:00
}
2013-01-25 02:11:09 -04:00
2013-02-25 04:50:56 -04:00
//////////////////////////////////////////////////////////
// circle navigation controller
//////////////////////////////////////////////////////////
// circle_set_center -- set circle controller's center position and starting angle
static void
2013-03-22 05:38:07 -03:00
circle_set_center(const Vector3f current_position, float heading_in_radians)
2013-02-25 04:50:56 -04:00
{
2013-06-01 05:53:57 -03:00
float max_velocity;
2013-06-15 06:14:36 -03:00
float cir_radius = g.circle_radius * 100;
2013-06-01 05:53:57 -03:00
2013-03-22 05:38:07 -03:00
// set circle center to circle_radius ahead of current position
2013-06-15 06:14:36 -03:00
circle_center.x = current_position.x + cir_radius * cos_yaw;
circle_center.y = current_position.y + cir_radius * sin_yaw;
2013-02-25 04:50:56 -04:00
2013-03-22 05:38:07 -03:00
// if we are doing a panorama set the circle_angle to the current heading
2013-06-03 10:10:32 -03:00
if( g.circle_radius <= 0 ) {
2013-03-22 05:38:07 -03:00
circle_angle = heading_in_radians;
2013-06-01 05:53:57 -03:00
circle_angular_velocity_max = ToRad(g.circle_rate);
circle_angular_acceleration = circle_angular_velocity_max; // reach maximum yaw velocity in 1 second
2013-03-22 05:38:07 -03:00
}else{
// set starting angle to current heading - 180 degrees
2013-06-01 05:53:57 -03:00
circle_angle = wrap_PI(heading_in_radians-PI);
// calculate max velocity based on waypoint speed ensuring we do not use more than half our max acceleration for accelerating towards the center of the circle
2013-10-29 01:28:27 -03:00
max_velocity = min(wp_nav.get_horizontal_velocity(), safe_sqrt(0.5f*wp_nav.get_waypoint_acceleration()*g.circle_radius*100.0f));
2013-06-01 05:53:57 -03:00
// angular_velocity in radians per second
2013-06-03 10:10:32 -03:00
circle_angular_velocity_max = max_velocity/((float)g.circle_radius * 100.0f);
circle_angular_velocity_max = constrain_float(ToRad(g.circle_rate),-circle_angular_velocity_max,circle_angular_velocity_max);
// angular_velocity in radians per second
2013-07-10 07:44:24 -03:00
circle_angular_acceleration = wp_nav.get_waypoint_acceleration()/((float)g.circle_radius * 100);
2013-06-03 10:10:32 -03:00
if (g.circle_rate < 0.0f) {
circle_angular_acceleration = -circle_angular_acceleration;
}
2013-02-25 04:50:56 -04:00
}
// initialise other variables
circle_angle_total = 0;
2013-06-01 05:53:57 -03:00
circle_angular_velocity = 0;
2013-06-15 06:14:36 -03:00
// initialise loiter target. Note: feed forward velocity set to zero
wp_nav.init_loiter_target(current_position, Vector3f(0,0,0));
2013-02-25 04:50:56 -04:00
}
2013-03-22 05:38:07 -03:00
// update_circle - circle position controller's main call which in turn calls loiter controller with updated target position
2013-02-25 04:50:56 -04:00
static void
2013-08-27 23:33:10 -03:00
update_circle()
2013-02-25 04:50:56 -04:00
{
2013-08-27 23:33:10 -03:00
static float last_update; // time of last circle call
2013-02-25 04:50:56 -04:00
2013-08-27 23:33:10 -03:00
// calculate dt
uint32_t now = hal.scheduler->millis();
float dt = (now - last_update) / 1000.0f;
// ensure enough time has passed since the last iteration
if (dt >= 0.095f) {
float cir_radius = g.circle_radius * 100;
Vector3f circle_target;
// range check dt
if (dt >= 1.0f) {
dt = 0;
2013-06-03 10:10:32 -03:00
}
2013-08-27 23:33:10 -03:00
// update time of circle call
last_update = now;
// ramp up angular velocity to maximum
if (g.circle_rate >= 0) {
if (circle_angular_velocity < circle_angular_velocity_max) {
circle_angular_velocity += circle_angular_acceleration * dt;
circle_angular_velocity = constrain_float(circle_angular_velocity, 0, circle_angular_velocity_max);
}
}else{
if (circle_angular_velocity > circle_angular_velocity_max) {
circle_angular_velocity += circle_angular_acceleration * dt;
circle_angular_velocity = constrain_float(circle_angular_velocity, circle_angular_velocity_max, 0);
}
2013-06-03 10:10:32 -03:00
}
2013-02-25 04:50:56 -04:00
2013-08-27 23:33:10 -03:00
// update the target angle
circle_angle += circle_angular_velocity * dt;
circle_angle = wrap_PI(circle_angle);
2013-06-01 05:53:57 -03:00
2013-08-27 23:33:10 -03:00
// update the total angle travelled
circle_angle_total += circle_angular_velocity * dt;
2013-02-25 04:50:56 -04:00
2013-08-27 23:33:10 -03:00
// if the circle_radius is zero we are doing panorama so no need to update loiter target
if( g.circle_radius != 0.0 ) {
// calculate target position
circle_target.x = circle_center.x + cir_radius * cosf(-circle_angle);
circle_target.y = circle_center.y - cir_radius * sinf(-circle_angle);
circle_target.z = wp_nav.get_desired_alt();
2013-02-25 04:50:56 -04:00
2013-08-27 23:33:10 -03:00
// re-use loiter position controller
wp_nav.set_loiter_target(circle_target);
}
2013-03-22 05:38:07 -03:00
}
2013-03-20 10:29:08 -03:00
// call loiter controller
wp_nav.update_loiter();
2013-03-28 23:14:31 -03:00
}