@ -108,8 +108,8 @@ struct planned_path_segments_s {
@@ -108,8 +108,8 @@ struct planned_path_segments_s {
double end_lon ;
float radius ; // Radius of arc
float arc_start_bearing ; // Bearing from center to start of arc
float arc_sweep ; // Angle (radians) swept out by arc around center.
// Positive for clockwise, negative for counter-clockwise
float arc_sweep ; // Angle (radians) swept out by arc around center.
// Positive for clockwise, negative for counter-clockwise
} ;
@ -176,241 +176,241 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc
@@ -176,241 +176,241 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc
int fixedwing_pos_control_thread_main ( int argc , char * argv [ ] )
{
/* read arguments */
bool verbose = false ;
bool verbose = false ;
for ( int i = 1 ; i < argc ; i + + ) {
if ( strcmp ( argv [ i ] , " -v " ) = = 0 | | strcmp ( argv [ i ] , " --verbose " ) = = 0 ) {
verbose = true ;
}
for ( int i = 1 ; i < argc ; i + + ) {
if ( strcmp ( argv [ i ] , " -v " ) = = 0 | | strcmp ( argv [ i ] , " --verbose " ) = = 0 ) {
verbose = true ;
}
}
/* welcome user */
printf ( " [fixedwing pos control] started \n " ) ;
/* declare and safely initialize all structs */
struct vehicle_global_position_s global_pos ;
memset ( & global_pos , 0 , sizeof ( global_pos ) ) ;
struct vehicle_global_position_s start_pos ; // Temporary variable, replace with
memset ( & start_pos , 0 , sizeof ( start_pos ) ) ; // previous waypoint when available
struct vehicle_global_position_setpoint_s global_setpoint ;
memset ( & global_setpoint , 0 , sizeof ( global_setpoint ) ) ;
struct vehicle_attitude_s att ;
memset ( & att , 0 , sizeof ( att ) ) ;
struct crosstrack_error_s xtrack_err ;
memset ( & xtrack_err , 0 , sizeof ( xtrack_err ) ) ;
struct parameter_update_s param_update ;
memset ( & param_update , 0 , sizeof ( param_update ) ) ;
/* output structs */
struct vehicle_attitude_setpoint_s attitude_setpoint ;
memset ( & attitude_setpoint , 0 , sizeof ( attitude_setpoint ) ) ;
/* publish attitude setpoint */
attitude_setpoint . roll_body = 0.0f ;
attitude_setpoint . pitch_body = 0.0f ;
attitude_setpoint . yaw_body = 0.0f ;
attitude_setpoint . thrust = 0.0f ;
orb_advert_t attitude_setpoint_pub = orb_advertise ( ORB_ID ( vehicle_attitude_setpoint ) , & attitude_setpoint ) ;
/* subscribe */
int global_pos_sub = orb_subscribe ( ORB_ID ( vehicle_global_position ) ) ;
int global_setpoint_sub = orb_subscribe ( ORB_ID ( vehicle_global_position_setpoint ) ) ;
int att_sub = orb_subscribe ( ORB_ID ( vehicle_attitude ) ) ;
int param_sub = orb_subscribe ( ORB_ID ( parameter_update ) ) ;
/* Setup of loop */
struct pollfd fds [ 2 ] = {
{ . fd = param_sub , . events = POLLIN } ,
{ . fd = att_sub , . events = POLLIN }
} ;
bool global_sp_updated_set_once = false ;
float psi_track = 0.0f ;
int counter = 0 ;
struct fw_pos_control_params p ;
struct fw_pos_control_param_handles h ;
PID_t heading_controller ;
PID_t heading_rate_controller ;
PID_t offtrack_controller ;
PID_t altitude_controller ;
parameters_init ( & h ) ;
parameters_update ( & h , & p ) ;
pid_init ( & heading_controller , p . heading_p , 0.0f , 0.0f , 0.0f , 10000.0f , PID_MODE_DERIVATIV_NONE ) ; //arbitrary high limit
pid_init ( & heading_rate_controller , p . headingr_p , p . headingr_i , 0.0f , 0.0f , p . roll_lim , PID_MODE_DERIVATIV_NONE ) ;
pid_init ( & altitude_controller , p . altitude_p , 0.0f , 0.0f , 0.0f , p . pitch_lim , PID_MODE_DERIVATIV_NONE ) ;
pid_init ( & offtrack_controller , p . xtrack_p , 0.0f , 0.0f , 0.0f , 60.0f * M_DEG_TO_RAD , PID_MODE_DERIVATIV_NONE ) ; //TODO: remove hardcoded value
/* error and performance monitoring */
perf_counter_t fw_interval_perf = perf_alloc ( PC_INTERVAL , " fixedwing_pos_control_interval " ) ;
perf_counter_t fw_err_perf = perf_alloc ( PC_COUNT , " fixedwing_pos_control_err " ) ;
while ( ! thread_should_exit ) {
/* wait for a sensor update, check for exit condition every 500 ms */
int ret = poll ( fds , 2 , 500 ) ;
if ( ret < 0 ) {
/* poll error, count it in perf */
perf_count ( fw_err_perf ) ;
} else if ( ret = = 0 ) {
/* no return value, ignore */
} else {
/* only update parameters if they changed */
if ( fds [ 0 ] . revents & POLLIN ) {
/* read from param to clear updated flag */
struct parameter_update_s update ;
orb_copy ( ORB_ID ( parameter_update ) , param_sub , & update ) ;
/* update parameters from storage */
parameters_update ( & h , & p ) ;
pid_set_parameters ( & heading_controller , p . heading_p , 0 , 0 , 0 , 10000.0f ) ; //arbitrary high limit
pid_set_parameters ( & heading_rate_controller , p . headingr_p , p . headingr_i , 0 , 0 , p . roll_lim ) ;
pid_set_parameters ( & altitude_controller , p . altitude_p , 0 , 0 , 0 , p . pitch_lim ) ;
pid_set_parameters ( & offtrack_controller , p . xtrack_p , 0 , 0 , 0 , 60.0f * M_DEG_TO_RAD ) ; //TODO: remove hardcoded value
}
/* only run controller if attitude changed */
if ( fds [ 1 ] . revents & POLLIN ) {
static uint64_t last_run = 0 ;
const float deltaT = ( hrt_absolute_time ( ) - last_run ) / 1000000.0f ;
last_run = hrt_absolute_time ( ) ;
/* welcome user */
printf ( " [fixedwing pos control] started \n " ) ;
/* declare and safely initialize all structs */
struct vehicle_global_position_s global_pos ;
memset ( & global_pos , 0 , sizeof ( global_pos ) ) ;
struct vehicle_global_position_s start_pos ; // Temporary variable, replace with
memset ( & start_pos , 0 , sizeof ( start_pos ) ) ; // previous waypoint when available
struct vehicle_global_position_setpoint_s global_setpoint ;
memset ( & global_setpoint , 0 , sizeof ( global_setpoint ) ) ;
struct vehicle_attitude_s att ;
memset ( & att , 0 , sizeof ( att ) ) ;
struct crosstrack_error_s xtrack_err ;
memset ( & xtrack_err , 0 , sizeof ( xtrack_err ) ) ;
struct parameter_update_s param_update ;
memset ( & param_update , 0 , sizeof ( param_update ) ) ;
/* output structs */
struct vehicle_attitude_setpoint_s attitude_setpoint ;
memset ( & attitude_setpoint , 0 , sizeof ( attitude_setpoint ) ) ;
/* publish attitude setpoint */
attitude_setpoint . roll_body = 0.0f ;
attitude_setpoint . pitch_body = 0.0f ;
attitude_setpoint . yaw_body = 0.0f ;
attitude_setpoint . thrust = 0.0f ;
orb_advert_t attitude_setpoint_pub = orb_advertise ( ORB_ID ( vehicle_attitude_setpoint ) , & attitude_setpoint ) ;
/* subscribe */
int global_pos_sub = orb_subscribe ( ORB_ID ( vehicle_global_position ) ) ;
int global_setpoint_sub = orb_subscribe ( ORB_ID ( vehicle_global_position_setpoint ) ) ;
int att_sub = orb_subscribe ( ORB_ID ( vehicle_attitude ) ) ;
int param_sub = orb_subscribe ( ORB_ID ( parameter_update ) ) ;
/* Setup of loop */
struct pollfd fds [ 2 ] = {
{ . fd = param_sub , . events = POLLIN } ,
{ . fd = att_sub , . events = POLLIN }
} ;
bool global_sp_updated_set_once = false ;
float psi_track = 0.0f ;
int counter = 0 ;
struct fw_pos_control_params p ;
struct fw_pos_control_param_handles h ;
PID_t heading_controller ;
PID_t heading_rate_controller ;
PID_t offtrack_controller ;
PID_t altitude_controller ;
parameters_init ( & h ) ;
parameters_update ( & h , & p ) ;
pid_init ( & heading_controller , p . heading_p , 0.0f , 0.0f , 0.0f , 10000.0f , PID_MODE_DERIVATIV_NONE ) ; //arbitrary high limit
pid_init ( & heading_rate_controller , p . headingr_p , p . headingr_i , 0.0f , 0.0f , p . roll_lim , PID_MODE_DERIVATIV_NONE ) ;
pid_init ( & altitude_controller , p . altitude_p , 0.0f , 0.0f , 0.0f , p . pitch_lim , PID_MODE_DERIVATIV_NONE ) ;
pid_init ( & offtrack_controller , p . xtrack_p , 0.0f , 0.0f , 0.0f , 60.0f * M_DEG_TO_RAD , PID_MODE_DERIVATIV_NONE ) ; //TODO: remove hardcoded value
/* error and performance monitoring */
perf_counter_t fw_interval_perf = perf_alloc ( PC_INTERVAL , " fixedwing_pos_control_interval " ) ;
perf_counter_t fw_err_perf = perf_alloc ( PC_COUNT , " fixedwing_pos_control_err " ) ;
while ( ! thread_should_exit )
{
/* wait for a sensor update, check for exit condition every 500 ms */
int ret = poll ( fds , 2 , 500 ) ;
if ( ret < 0 ) {
/* poll error, count it in perf */
perf_count ( fw_err_perf ) ;
} else if ( ret = = 0 ) {
/* no return value, ignore */
} else {
/* only update parameters if they changed */
if ( fds [ 0 ] . revents & POLLIN ) {
/* read from param to clear updated flag */
struct parameter_update_s update ;
orb_copy ( ORB_ID ( parameter_update ) , param_sub , & update ) ;
/* update parameters from storage */
parameters_update ( & h , & p ) ;
pid_set_parameters ( & heading_controller , p . heading_p , 0 , 0 , 0 , 10000.0f ) ; //arbitrary high limit
pid_set_parameters ( & heading_rate_controller , p . headingr_p , p . headingr_i , 0 , 0 , p . roll_lim ) ;
pid_set_parameters ( & altitude_controller , p . altitude_p , 0 , 0 , 0 , p . pitch_lim ) ;
pid_set_parameters ( & offtrack_controller , p . xtrack_p , 0 , 0 , 0 , 60.0f * M_DEG_TO_RAD ) ; //TODO: remove hardcoded value
/* check if there is a new position or setpoint */
bool pos_updated ;
orb_check ( global_pos_sub , & pos_updated ) ;
bool global_sp_updated ;
orb_check ( global_setpoint_sub , & global_sp_updated ) ;
/* load local copies */
orb_copy ( ORB_ID ( vehicle_attitude ) , att_sub , & att ) ;
if ( pos_updated ) {
orb_copy ( ORB_ID ( vehicle_global_position ) , global_pos_sub , & global_pos ) ;
}
/* only run controller if attitude changed */
if ( fds [ 1 ] . revents & POLLIN ) {
if ( global_sp_updated ) {
orb_copy ( ORB_ID ( vehicle_global_position_setpoint ) , global_setpoint_sub , & global_setpoint ) ;
start_pos = global_pos ; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
global_sp_updated_set_once = true ;
psi_track = get_bearing_to_next_waypoint ( ( double ) global_pos . lat / ( double ) 1e7 d , ( double ) global_pos . lon / ( double ) 1e7 d ,
( double ) global_setpoint . lat / ( double ) 1e7 d , ( double ) global_setpoint . lon / ( double ) 1e7 d ) ;
printf ( " next wp direction: %0.4f \n " , ( double ) psi_track ) ;
}
static uint64_t last_run = 0 ;
const float deltaT = ( hrt_absolute_time ( ) - last_run ) / 1000000.0f ;
last_run = hrt_absolute_time ( ) ;
/* Simple Horizontal Control */
if ( global_sp_updated_set_once ) {
// if (counter % 100 == 0)
// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
/* check if there is a new position or setpoint */
bool pos_updated ;
orb_check ( global_pos_sub , & pos_updated ) ;
bool global_sp_updated ;
orb_check ( global_setpoint_sub , & global_sp_update d) ;
/* calculate crosstrack error */
// Only the case of a straight line track following handled so far
int distance_res = get_distance_to_line ( & xtrack_err , ( double ) global_pos . lat / ( double ) 1e7 d , ( double ) global_pos . lon / ( double ) 1e7 d ,
( double ) start_pos . lat / ( double ) 1e7 d , ( double ) start_pos . lon / ( double ) 1e7 d ,
( double ) global_setpoint . lat / ( double ) 1e7 d , ( double ) global_setpoint . lon / ( double ) 1e7 d ) ;
/* load local copies */
orb_copy ( ORB_ID ( vehicle_attitude ) , att_sub , & att ) ;
// XXX what is xtrack_err.past_end?
if ( distance_res = = OK /*&& !xtrack_err.past_end*/ ) {
if ( pos_updated ) {
orb_copy ( ORB_ID ( vehicle_global_position ) , global_pos_sub , & global_pos ) ;
}
float delta_psi_c = pid_calculate ( & offtrack_controller , 0 , xtrack_err . distance , 0.0f , 0.0f ) ; //p.xtrack_p * xtrack_err.distance
if ( global_sp_updated ) {
orb_copy ( ORB_ID ( vehicle_global_position_setpoint ) , global_setpoint_sub , & global_setpoint ) ;
start_pos = global_pos ; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
global_sp_updated_set_once = true ;
psi_track = get_bearing_to_next_waypoint ( ( double ) global_pos . lat / ( double ) 1e7 d , ( double ) global_pos . lon / ( double ) 1e7 d ,
( double ) global_setpoint . lat / ( double ) 1e7 d , ( double ) global_setpoint . lon / ( double ) 1e7 d ) ;
printf ( " next wp direction: %0.4f \n " , ( double ) psi_track ) ;
}
float psi_c = psi_track + delta_psi_c ;
float psi_e = psi_c - att . yaw ;
/* wrap difference back onto -pi..pi range */
psi_e = _wrap_pi ( psi_e ) ;
if ( verbose ) {
printf ( " xtrack_err.distance %.4f " , ( double ) xtrack_err . distance ) ;
printf ( " delta_psi_c %.4f " , ( double ) delta_psi_c ) ;
printf ( " psi_c %.4f " , ( double ) psi_c ) ;
printf ( " att.yaw %.4f " , ( double ) att . yaw ) ;
printf ( " psi_e %.4f " , ( double ) psi_e ) ;
}
/* Simple Horizontal Control */
if ( global_sp_updated_set_once )
{
// if (counter % 100 == 0)
// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
/* calculate crosstrack error */
// Only the case of a straight line track following handled so far
int distance_res = get_distance_to_line ( & xtrack_err , ( double ) global_pos . lat / ( double ) 1e7 d , ( double ) global_pos . lon / ( double ) 1e7 d ,
( double ) start_pos . lat / ( double ) 1e7 d , ( double ) start_pos . lon / ( double ) 1e7 d ,
( double ) global_setpoint . lat / ( double ) 1e7 d , ( double ) global_setpoint . lon / ( double ) 1e7 d ) ;
// XXX what is xtrack_err.past_end?
if ( distance_res = = OK /*&& !xtrack_err.past_end*/ ) {
float delta_psi_c = pid_calculate ( & offtrack_controller , 0 , xtrack_err . distance , 0.0f , 0.0f ) ; //p.xtrack_p * xtrack_err.distance
float psi_c = psi_track + delta_psi_c ;
float psi_e = psi_c - att . yaw ;
/* wrap difference back onto -pi..pi range */
psi_e = _wrap_pi ( psi_e ) ;
if ( verbose ) {
printf ( " xtrack_err.distance %.4f " , ( double ) xtrack_err . distance ) ;
printf ( " delta_psi_c %.4f " , ( double ) delta_psi_c ) ;
printf ( " psi_c %.4f " , ( double ) psi_c ) ;
printf ( " att.yaw %.4f " , ( double ) att . yaw ) ;
printf ( " psi_e %.4f " , ( double ) psi_e ) ;
}
/* calculate roll setpoint, do this artificially around zero */
float delta_psi_rate_c = pid_calculate ( & heading_controller , psi_e , 0.0f , 0.0f , 0.0f ) ;
float psi_rate_track = 0 ; //=V_gr/r_track , this will be needed for implementation of arc following
float psi_rate_c = delta_psi_rate_c + psi_rate_track ;
/* limit turn rate */
if ( psi_rate_c > p . headingr_lim ) {
psi_rate_c = p . headingr_lim ;
} else if ( psi_rate_c < - p . headingr_lim ) {
psi_rate_c = - p . headingr_lim ;
}
float psi_rate_e = psi_rate_c - att . yawspeed ;
// XXX sanity check: Assume 10 m/s stall speed and no stall condition
float ground_speed = sqrtf ( global_pos . vx * global_pos . vx + global_pos . vy * global_pos . vy ) ;
if ( ground_speed < 10.0f ) {
ground_speed = 10.0f ;
}
float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f ; //* V_gr / g
attitude_setpoint . roll_body = pid_calculate ( & heading_rate_controller , psi_rate_e_scaled , 0.0f , 0.0f , deltaT ) ;
if ( verbose ) {
printf ( " psi_rate_c %.4f " , ( double ) psi_rate_c ) ;
printf ( " psi_rate_e_scaled %.4f " , ( double ) psi_rate_e_scaled ) ;
printf ( " rollbody %.4f \n " , ( double ) attitude_setpoint . roll_body ) ;
}
if ( verbose & & counter % 100 = = 0 )
printf ( " xtrack_err.distance: %0.4f, delta_psi_c: %0.4f \n " , xtrack_err . distance , delta_psi_c ) ;
} else {
if ( verbose & & counter % 100 = = 0 )
printf ( " distance_res: %d, past_end %d \n " , distance_res , xtrack_err . past_end ) ;
/* calculate roll setpoint, do this artificially around zero */
float delta_psi_rate_c = pid_calculate ( & heading_controller , psi_e , 0.0f , 0.0f , 0.0f ) ;
float psi_rate_track = 0 ; //=V_gr/r_track , this will be needed for implementation of arc following
float psi_rate_c = delta_psi_rate_c + psi_rate_track ;
/* limit turn rate */
if ( psi_rate_c > p . headingr_lim ) {
psi_rate_c = p . headingr_lim ;
} else if ( psi_rate_c < - p . headingr_lim ) {
psi_rate_c = - p . headingr_lim ;
}
/* Very simple Altitude Control */
if ( pos_updated )
{
float psi_rate_e = psi_rate_c - att . yawspeed ;
//TODO: take care of relative vs. ab. altitude
attitude_setpoint . pitch_body = pid_calculate ( & altitude_controller , global_setpoint . altitude , global_pos . alt , 0.0f , 0.0f ) ;
// XXX sanity check: Assume 10 m/s stall speed and no stall condition
float ground_speed = sqrtf ( global_pos . vx * global_pos . vx + global_pos . vy * global_pos . vy ) ;
if ( ground_speed < 10.0f ) {
ground_speed = 10.0f ;
}
// XXX need speed control
attitude_setpoint . thrust = 0.7f ;
float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f ; //* V_gr / g
/* publish the attitude setpoint */
orb_publish ( ORB_ID ( vehicle_attitude_setpoint ) , attitude_setpoint_pub , & attitude_setpoint ) ;
attitude_setpoint . roll_body = pid_calculate ( & heading_rate_controller , psi_rate_e_scaled , 0.0f , 0.0f , deltaT ) ;
/* measure in what intervals the controller runs */
perf_count ( fw_interval_perf ) ;
if ( verbose ) {
printf ( " psi_rate_c %.4f " , ( double ) psi_rate_c ) ;
printf ( " psi_rate_e_scaled %.4f " , ( double ) psi_rate_e_scaled ) ;
printf ( " rollbody %.4f \n " , ( double ) attitude_setpoint . roll_body ) ;
}
counter + + ;
if ( verbose & & counter % 100 = = 0 )
printf ( " xtrack_err.distance: %0.4f, delta_psi_c: %0.4f \n " , xtrack_err . distance , delta_psi_c ) ;
} else {
// XXX no setpoint, decent default needed (loiter?)
if ( verbose & & counter % 100 = = 0 )
printf ( " distance_res: %d, past_end %d \n " , distance_res , xtrack_err . past_end ) ;
}
/* Very simple Altitude Control */
if ( pos_updated ) {
//TODO: take care of relative vs. ab. altitude
attitude_setpoint . pitch_body = pid_calculate ( & altitude_controller , global_setpoint . altitude , global_pos . alt , 0.0f , 0.0f ) ;
}
// XXX need speed control
attitude_setpoint . thrust = 0.7f ;
/* publish the attitude setpoint */
orb_publish ( ORB_ID ( vehicle_attitude_setpoint ) , attitude_setpoint_pub , & attitude_setpoint ) ;
/* measure in what intervals the controller runs */
perf_count ( fw_interval_perf ) ;
counter + + ;
} else {
// XXX no setpoint, decent default needed (loiter?)
}
}
}
}
printf ( " [fixedwing_pos_control] exiting. \n " ) ;
thread_running = false ;
printf ( " [fixedwing_pos_control] exiting. \n " ) ;
thread_running = false ;
close ( attitude_setpoint_pub ) ;
close ( attitude_setpoint_pub ) ;
fflush ( stdout ) ;
exit ( 0 ) ;
fflush ( stdout ) ;
exit ( 0 ) ;
return 0 ;
return 0 ;
}
@ -421,6 +421,7 @@ usage(const char *reason)
@@ -421,6 +421,7 @@ usage(const char *reason)
{
if ( reason )
fprintf ( stderr , " %s \n " , reason ) ;
fprintf ( stderr , " usage: fixedwing_pos_control {start|stop|status} \n \n " ) ;
exit ( 1 ) ;
}
@ -465,9 +466,11 @@ int fixedwing_pos_control_main(int argc, char *argv[])
@@ -465,9 +466,11 @@ int fixedwing_pos_control_main(int argc, char *argv[])
if ( ! strcmp ( argv [ 1 ] , " status " ) ) {
if ( thread_running ) {
printf ( " \t fixedwing_pos_control is running \n " ) ;
} else {
printf ( " \t fixedwing_pos_control not started \n " ) ;
}
exit ( 0 ) ;
}
Lorenz Meier
12 years ago