From 6ff4520904daef1fa441fd467f048c42d286f2ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Nov 2012 15:19:22 +0100 Subject: [PATCH] Cleaned up PI wrapping code, still subject to testing --- .../fixedwing_att_control_att.c | 4 +- .../fixedwing_pos_control.h | 19 ---- .../fixedwing_pos_control_main.c | 96 +++++-------------- .../multirotor_att_control_main.c | 27 +++--- .../multirotor_attitude_control.c | 22 +++-- .../multirotor_attitude_control.h | 2 +- apps/sensors/sensors.cpp | 2 +- apps/systemlib/geo/geo.c | 76 +++++++-------- apps/systemlib/geo/geo.h | 38 ++++++-- apps/uORB/topics/vehicle_attitude_setpoint.h | 9 +- 10 files changed, 121 insertions(+), 174 deletions(-) diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c index 18b290f994..334b95226f 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_att.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c @@ -143,10 +143,10 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att } /* Roll (P) */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_tait_bryan, att->roll, 0, 0); + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); /* Pitch (P) */ - float pitch_sp_rollcompensation = att_sp->pitch_tait_bryan + p.pitch_roll_compensation_p * att_sp->roll_tait_bryan; + float pitch_sp_rollcompensation = att_sp->pitch_body + p.pitch_roll_compensation_p * att_sp->roll_body; rates_sp->pitch = pid_calculate(&pitch_controller, pitch_sp_rollcompensation, att->pitch, 0, 0); /* Yaw (from coordinated turn constraint or lateral force) */ diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control.h b/apps/fixedwing_pos_control/fixedwing_pos_control.h index f631c90a1b..90d717d9f5 100644 --- a/apps/fixedwing_pos_control/fixedwing_pos_control.h +++ b/apps/fixedwing_pos_control/fixedwing_pos_control.h @@ -42,26 +42,7 @@ #ifndef FIXEDWING_POS_CONTROL_H_ #define FIXEDWING_POS_CONTROL_H_ -#include -#include -#include -#include -#include -#endif - - -struct planned_path_segments_s { - bool segment_type; - double start_lat; // Start of line or center of arc - double start_lon; - double end_lat; - 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 _wrap180(float bearing); diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c index 4430489130..a70b9bd30d 100644 --- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -91,6 +91,19 @@ struct fw_pos_control_param_handles { }; +struct planned_path_segments_s { + bool segment_type; + double start_lat; // Start of line or center of arc + double start_lon; + double end_lat; + 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 +}; + + /* Prototypes */ /* Internal Prototypes */ static int parameters_init(struct fw_pos_control_param_handles *h); @@ -177,9 +190,9 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) memset(&attitude_setpoint, 0, sizeof(attitude_setpoint)); /* publish attitude setpoint */ - attitude_setpoint.roll_tait_bryan = 0.0f; - attitude_setpoint.pitch_tait_bryan = 0.0f; - attitude_setpoint.yaw_tait_bryan = 0.0f; + attitude_setpoint.roll_body = 0.0f; + attitude_setpoint.pitch_body = 0.0f; + attitude_setpoint.yaw_body = 0.0f; orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint); /* subscribe */ @@ -243,7 +256,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) global_sp_updated_set_once = true; psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - printf("psi_track: %0.4f\n", psi_track); + printf("psi_track: %0.4f\n", (double)psi_track); } /* Control */ @@ -265,21 +278,21 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards - if(delta_psi_c > 60.0f*M_DEG_TO_RAD) - delta_psi_c = 60.0f*M_DEG_TO_RAD; + if(delta_psi_c > 60.0f*M_DEG_TO_RAD_F) + delta_psi_c = 60.0f*M_DEG_TO_RAD_F; - if(delta_psi_c < -60.0f*M_DEG_TO_RAD) - delta_psi_c = -60.0f*M_DEG_TO_RAD; + if(delta_psi_c < -60.0f*M_DEG_TO_RAD_F) + delta_psi_c = -60.0f*M_DEG_TO_RAD_F; float psi_c = psi_track + delta_psi_c; float psi_e = psi_c - att.yaw; /* shift error to prevent wrapping issues */ - psi_e = _wrapPI(psi_e); + psi_e = _wrap_pi(psi_e); /* calculate roll setpoint, do this artificially around zero */ - attitude_setpoint.roll_tait_bryan = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + attitude_setpoint.roll_body = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); // if (counter % 100 == 0) // printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c); @@ -296,7 +309,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) { //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_tait_bryan = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); } /*Publish the attitude setpoint */ @@ -378,64 +391,3 @@ int fixedwing_pos_control_main(int argc, char *argv[]) usage("unrecognized command"); exit(1); } - - -float _wrapPI(float bearing) -{ - - while (bearing > M_PI_F) { - bearing = bearing - M_TWOPI_F; - } - - while (bearing <= -M_PI_F) { - bearing = bearing + M_TWOPI_F; - } - - return bearing; -} - -float _wrap2PI(float bearing) -{ - - while (bearing >= M_TWOPI_F) { - bearing = bearing - M_TWOPI_F; - } - - while (bearing < 0.0f) { - bearing = bearing + M_TWOPI_F; - } - - return bearing; -} - -float _wrap180(float bearing) -{ - - while (bearing > 180.0f) { - bearing = bearing - 360.0f; - } - - while (bearing <= -180.0f) { - bearing = bearing + 360.0f; - } - - return bearing; -} - -float _wrap360(float bearing) -{ - - while (bearing >= 360.0f) { - bearing = bearing - 360.0f; - } - - while (bearing < 0.0f) { - bearing = bearing + 360.0f; - } - - return bearing; -} - - - - diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 91439d36d5..10d155ffc8 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -252,7 +252,7 @@ mc_thread_main(int argc, char *argv[]) if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || state.flag_control_manual_enabled != flag_control_manual_enabled || state.flag_system_armed != flag_system_armed) { - att_sp.yaw_tait_bryan = att.yaw; + att_sp.yaw_body = att.yaw; } static bool rc_loss_first_time = true; @@ -283,29 +283,28 @@ mc_thread_main(int argc, char *argv[]) att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; + /* set attitude if arming */ + if (!flag_control_attitude_enabled && state.flag_system_armed) { + att_sp.yaw_body = att.yaw; + } + /* only move setpoint if manual input is != 0 */ if(manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_POS) { - // XXX turn into param - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { - att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.0025f; - } else if (manual.throttle <= 0.3f) { - att_sp.yaw_body = att.yaw; - } - control_yaw_position = true; - } else if (manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_RATE) { if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { rates_sp.yaw = manual.yaw; control_yaw_position = false; first_time_after_yaw_speed_control = true; } else { - rates_sp.yaw = 0.0f; - if(first_time_after_yaw_speed_control) { - att_sp.yaw_tait_bryan = att.yaw; + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; first_time_after_yaw_speed_control = false; } control_yaw_position = true; } + } else if (manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_RATE) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; } att_sp.thrust = manual.throttle; @@ -330,9 +329,7 @@ mc_thread_main(int argc, char *argv[]) /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ if (state.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL, control_yaw_position); - - + multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 87eeaced5a..e94d7c55d8 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -158,7 +158,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators, bool control_yaw_position) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position) { static uint64_t last_run = 0; static uint64_t last_input = 0; @@ -201,8 +201,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* update parameters from storage */ parameters_update(&h, &p); - //printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); - /* apply parameters */ pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); @@ -220,15 +218,19 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s if(control_yaw_position) { /* control yaw rate */ - //rates_sp->yaw = p.yaw_p * atan2f(cosf(att->yaw - att_sp->yaw_tait_bryan), sinf(att->yaw - att_sp->yaw_tait_bryan)) - (p.yaw_d * att->yawspeed); - yaw_error = att->yaw - att_sp->yaw_tait_bryan; - if ((double)yaw_error > M_PI) { - yaw_error -= M_PI; - } else if ((double)yaw_error < -M_PI) { - yaw_error += M_PI; + + /* positive error: rotate to right, negative error, rotate to left (NED frame) */ + // yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw); + + yaw_error = att_sp->yaw_body - att->yaw; + + if (yaw_error > M_PI_F) { + yaw_error -= M_TWOPI_F; + } else if (yaw_error < -M_PI_F) { + yaw_error += M_TWOPI_F; } - rates_sp->yaw = - p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed); + rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed); } rates_sp->thrust = att_sp->thrust; diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h index f9003e9b4c..abc94d6170 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.h +++ b/apps/multirotor_att_control/multirotor_attitude_control.h @@ -52,6 +52,6 @@ #include void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *, bool control_yaw_position); + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position); #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index aa1362d3a3..466284a1b4 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1059,7 +1059,7 @@ Sensors::task_main() /* advertise the manual_control topic */ struct manual_control_setpoint_s manual_control; - manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_RATE; + manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_POS; manual_control.roll = 0.0f; manual_control.pitch = 0.0f; manual_control.yaw = 0.0f; diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c index 2f4b37e79e..6746e8ff36 100644 --- a/apps/systemlib/geo/geo.c +++ b/apps/systemlib/geo/geo.c @@ -60,14 +60,7 @@ static double cos_phi_1; static double lambda_0; static double scale; -/** - * Initializes the map transformation. - * - * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -__EXPORT static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ phi_1 = lat_0 / 180.0 * M_PI; @@ -105,14 +98,7 @@ __EXPORT static void map_projection_init(double lat_0, double lon_0) //lat_0, lo } -/** - * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane - * @param x north - * @param y east - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -__EXPORT static void map_projection_project(double lat, double lon, float *x, float *y) +__EXPORT void map_projection_project(double lat, double lon, float *x, float *y) { /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ double phi = lat / 180.0 * M_PI; @@ -135,15 +121,7 @@ __EXPORT static void map_projection_project(double lat, double lon, float *x, fl // printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0); } -/** - * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system - * - * @param x north - * @param y east - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -__EXPORT static void map_projection_reproject(float x, float y, double *lat, double *lon) +__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon) { /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ @@ -228,7 +206,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub /* conscious mix of double and float trig function to maximize speed and efficiency */ float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); - theta = _wrapPI(theta); + theta = _wrap_pi(theta); return theta; } @@ -257,7 +235,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); bearing_diff = bearing_track - bearing_end; - bearing_diff = _wrapPI(bearing_diff); + bearing_diff = _wrap_pi(bearing_diff); // Return past_end = true if past end point of line if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { @@ -270,10 +248,10 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, crosstrack_error->distance = (dist_to_end) * sin(bearing_diff); if (sin(bearing_diff) >= 0) { - crosstrack_error->bearing = _wrapPI(bearing_track - M_PI_2_F); + crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F); } else { - crosstrack_error->bearing = _wrapPI(bearing_track + M_PI_2_F); + crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F); } return_value = OK; @@ -380,22 +358,36 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d return return_value; } -float _wrapPI(float bearing) +__EXPORT float _wrap_pi(float bearing) { - - while (bearing > M_PI_F) { - bearing = bearing - M_TWOPI_F; + /* value is inf or NaN */ + if (!isfinite(bearing) || bearing == 0) { + return bearing; } - while (bearing <= -M_PI_F) { - bearing = bearing + M_TWOPI_F; + int c = 0; + + while (bearing > M_PI_F && c < 30) { + bearing -= M_TWOPI_F; + c++; + } + + c = 0; + + while (bearing <= -M_PI_F && c < 30) { + bearing += M_TWOPI_F; + c++; } return bearing; } -float _wrap2PI(float bearing) +__EXPORT float _wrap_2pi(float bearing) { + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } while (bearing >= M_TWOPI_F) { bearing = bearing - M_TWOPI_F; @@ -408,8 +400,12 @@ float _wrap2PI(float bearing) return bearing; } -float _wrap180(float bearing) +__EXPORT float _wrap_180(float bearing) { + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } while (bearing > 180.0f) { bearing = bearing - 360.0f; @@ -422,8 +418,12 @@ float _wrap180(float bearing) return bearing; } -float _wrap360(float bearing) +__EXPORT float _wrap_360(float bearing) { + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } while (bearing >= 360.0f) { bearing = bearing - 360.0f; diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h index 7aad79a8c7..0c0b5c533e 100644 --- a/apps/systemlib/geo/geo.h +++ b/apps/systemlib/geo/geo.h @@ -54,24 +54,44 @@ struct crosstrack_error_s { float bearing; // Bearing in radians to closest point on line/arc } ; -__EXPORT static void map_projection_init(double lat_0, double lon_0); +/** + * Initializes the map transformation. + * + * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT void map_projection_init(double lat_0, double lon_0); -__EXPORT static void map_projection_project(double lat, double lon, float *x, float *y); +/** + * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT void map_projection_project(double lat, double lon, float *x, float *y); -__EXPORT static void map_projection_reproject(float x, float y, double *lat, double *lon); +/** + * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system + * + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon); __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -// - __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, float radius, float arc_start_bearing, float arc_sweep); -float _wrap180(float bearing); -float _wrap360(float bearing); -float _wrapPI(float bearing); -float _wrap2PI(float bearing); +__EXPORT float _wrap_180(float bearing); +__EXPORT float _wrap_360(float bearing); +__EXPORT float _wrap_pi(float bearing); +__EXPORT float _wrap_2pi(float bearing); diff --git a/apps/uORB/topics/vehicle_attitude_setpoint.h b/apps/uORB/topics/vehicle_attitude_setpoint.h index 8663846fc9..a7cda34a83 100644 --- a/apps/uORB/topics/vehicle_attitude_setpoint.h +++ b/apps/uORB/topics/vehicle_attitude_setpoint.h @@ -56,18 +56,13 @@ struct vehicle_attitude_setpoint_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - float roll_tait_bryan; /**< Tait-Bryan angle in NED frame */ - float pitch_tait_bryan; /**< Tait-Bryan angle in NED frame */ - float yaw_tait_bryan; /**< Tait-Bryan angle in NED frame */ - //float tait_bryan_valid; /**< Set to true if Tait-Bryan angles are valid */ - float roll_body; /**< body angle in NED frame */ float pitch_body; /**< body angle in NED frame */ float yaw_body; /**< body angle in NED frame */ //float body_valid; /**< Set to true if body angles are valid */ - //float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ - //bool R_valid; /**< Set to true if rotation matrix is valid */ + float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ + bool R_valid; /**< Set to true if rotation matrix is valid */ float thrust; /**< Thrust in Newton the power system should generate */