forked from Archive/PX4-Autopilot
HIL fixed, fixedwing control fixes
This commit is contained in:
Ivan Ovinnikov
parent
2b09a7914f
commit
9536bfa3ca
|
|
@ -475,6 +475,9 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
|
|||
if ((current_system_state == SYSTEM_STATE_STANDBY) && (mode & MAV_MODE_FLAG_HIL_ENABLED)) {
|
||||
/* Enable HIL on request */
|
||||
current_status->mode |= MAV_MODE_FLAG_HIL_ENABLED;
|
||||
ret = OK;
|
||||
state_machine_publish(status_pub, current_status);
|
||||
printf("[commander] Enabling HIL\n");
|
||||
}
|
||||
|
||||
/* NEVER actually switch off HIL without reboot */
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@
|
|||
#include <arch/board/board.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <nuttx/spi.h>
|
||||
#include "../mix_and_link/mix_and_link.h"
|
||||
#include "../mix_and_link/mix_and_link.h" //TODO: add to Makefile
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
|
|
@ -135,9 +135,9 @@ typedef struct {
|
|||
|
||||
/* Next waypoint*/
|
||||
|
||||
double wp_x;
|
||||
double wp_y;
|
||||
double wp_z;
|
||||
float wp_x;
|
||||
float wp_y;
|
||||
float wp_z;
|
||||
|
||||
/* Setpoints */
|
||||
|
||||
|
|
@ -190,7 +190,7 @@ static float calc_pitch_elev(void);
|
|||
static float calc_yaw_rudder(float hdg);
|
||||
static float calc_throttle(void);
|
||||
static float calc_gnd_speed(void);
|
||||
static void get_parameters(void);
|
||||
static void get_parameters(plane_data_t *plane_data);
|
||||
static float calc_roll_setpoint(void);
|
||||
static float calc_pitch_setpoint(void);
|
||||
static float calc_throttle_setpoint(void);
|
||||
|
|
@ -294,25 +294,27 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float
|
|||
/**
|
||||
* Load parameters from global storage.
|
||||
*
|
||||
* @param plane_data Fixed wing data structure
|
||||
*
|
||||
* Fetches the current parameters from the global parameter storage and writes them
|
||||
* to the plane_data structure
|
||||
*/
|
||||
|
||||
static void get_parameters()
|
||||
static void get_parameters(plane_data_t * plane_data)
|
||||
{
|
||||
plane_data.Kp_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P];
|
||||
plane_data.Ki_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I];
|
||||
plane_data.Kd_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D];
|
||||
plane_data.Kp_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P];
|
||||
plane_data.Ki_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I];
|
||||
plane_data.Kd_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D];
|
||||
plane_data.intmax_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU];
|
||||
plane_data.intmax_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU];
|
||||
plane_data.airspeed = global_data_parameter_storage->pm.param_values[PARAM_AIRSPEED];
|
||||
plane_data.wp_x = global_data_parameter_storage->pm.param_values[PARAM_WPLON];
|
||||
plane_data.wp_y = global_data_parameter_storage->pm.param_values[PARAM_WPLAT];
|
||||
plane_data.wp_z = global_data_parameter_storage->pm.param_values[PARAM_WPALT];
|
||||
plane_data.mode = global_data_parameter_storage->pm.param_values[PARAM_FLIGHTMODE];
|
||||
plane_data->Kp_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P];
|
||||
plane_data->Ki_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I];
|
||||
plane_data->Kd_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D];
|
||||
plane_data->Kp_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P];
|
||||
plane_data->Ki_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I];
|
||||
plane_data->Kd_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D];
|
||||
plane_data->intmax_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU];
|
||||
plane_data->intmax_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU];
|
||||
plane_data->airspeed = global_data_parameter_storage->pm.param_values[PARAM_AIRSPEED];
|
||||
plane_data->wp_x = global_data_parameter_storage->pm.param_values[PARAM_WPLON];
|
||||
plane_data->wp_y = global_data_parameter_storage->pm.param_values[PARAM_WPLAT];
|
||||
plane_data->wp_z = global_data_parameter_storage->pm.param_values[PARAM_WPALT];
|
||||
plane_data->mode = global_data_parameter_storage->pm.param_values[PARAM_FLIGHTMODE];
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -412,7 +414,7 @@ static float calc_bearing()
|
|||
|
||||
static float calc_roll_ail()
|
||||
{
|
||||
float ret = pid((plane_data.roll_setpoint - plane_data.roll) / scaler, plane_data.rollspeed, PID_DT, PID_SCALER,
|
||||
float ret = pid((plane_data.roll_setpoint - plane_data.roll), plane_data.rollspeed, PID_DT, PID_SCALER,
|
||||
plane_data.Kp_att, plane_data.Ki_att, plane_data.Kd_att, plane_data.intmax_att);
|
||||
|
||||
if (ret < -1)
|
||||
|
|
@ -433,7 +435,7 @@ static float calc_roll_ail()
|
|||
*/
|
||||
static float calc_pitch_elev()
|
||||
{
|
||||
float ret = pid((plane_data.pitch_setpoint - plane_data.pitch) / scaler, plane_data.pitchspeed, PID_DT, PID_SCALER,
|
||||
float ret = pid((plane_data.pitch_setpoint - plane_data.pitch), plane_data.pitchspeed, PID_DT, PID_SCALER,
|
||||
plane_data.Kp_att, plane_data.Ki_att, plane_data.Kd_att, plane_data.intmax_att);
|
||||
|
||||
if (ret < -1)
|
||||
|
|
@ -455,7 +457,7 @@ static float calc_pitch_elev()
|
|||
*/
|
||||
static float calc_yaw_rudder(float hdg)
|
||||
{
|
||||
float ret = pid((plane_data.yaw - abs(hdg)) / scaler, plane_data.yawspeed, PID_DT, PID_SCALER,
|
||||
float ret = pid((plane_data.yaw - abs(hdg)), plane_data.yawspeed, PID_DT, PID_SCALER,
|
||||
plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos);
|
||||
|
||||
if (ret < -1)
|
||||
|
|
@ -596,7 +598,7 @@ static float calc_throttle_setpoint()
|
|||
|
||||
// if TAKEOFF full throttle
|
||||
if (plane_data.mode == TAKEOFF) {
|
||||
setpoint = 0.6f;
|
||||
setpoint = 60.0f;
|
||||
}
|
||||
|
||||
// if CRUISE - parameter value
|
||||
|
|
@ -771,36 +773,39 @@ int fixedwing_control_main(int argc, char *argv[])
|
|||
plane_data.yawspeed = att.yawspeed;
|
||||
|
||||
/* parameter values */
|
||||
get_parameters();
|
||||
get_parameters(&plane_data);
|
||||
|
||||
/* Attitude control part */
|
||||
|
||||
if (verbose && loopcounter % 20 == 0) {
|
||||
/******************************** DEBUG OUTPUT ************************************************************/
|
||||
|
||||
printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)plane_data.Kp_att, (int)plane_data.Ki_att,
|
||||
printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)plane_data.Kp_att, (int)plane_data.Ki_att,
|
||||
(int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos,
|
||||
(int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed,
|
||||
(int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z);
|
||||
(int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z,
|
||||
(int)global_data_parameter_storage->pm.param_values[PARAM_WPLON],
|
||||
(int)global_data_parameter_storage->pm.param_values[PARAM_WPLAT],
|
||||
(int)global_data_parameter_storage->pm.param_values[PARAM_WPALT]);
|
||||
|
||||
// printf("PITCH SETPOINT: %i\n", (int)plane_data.pitch_setpoint);
|
||||
// printf("ROLL SETPOINT: %i\n", (int)plane_data.roll_setpoint);
|
||||
// printf("THROTTLE SETPOINT: %i\n", (int)calc_throttle_setpoint());
|
||||
printf("PITCH SETPOINT: %i\n", (int)(100.0f*plane_data.pitch_setpoint));
|
||||
printf("ROLL SETPOINT: %i\n", (int)(100.0f*plane_data.roll_setpoint));
|
||||
printf("THROTTLE SETPOINT: %i\n", (int)(100.0f*calc_throttle_setpoint()));
|
||||
|
||||
// printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed()));
|
||||
|
||||
// printf("Current Altitude: %i\n\n", (int)plane_data.alt);
|
||||
|
||||
printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ",
|
||||
(int)(1000 * plane_data.roll), (int)(1000 * plane_data.pitch), (int)(1000 * plane_data.yaw),
|
||||
(int)(100 * plane_data.rollspeed), (int)(100 * plane_data.pitchspeed), (int)(100 * plane_data.yawspeed));
|
||||
(int)(180.0f * plane_data.roll/F_M_PI), (int)(180.0f * plane_data.pitch/F_M_PI), (int)(180.0f * plane_data.yaw/F_M_PI),
|
||||
(int)(180.0f * plane_data.rollspeed/F_M_PI), (int)(180.0f * plane_data.pitchspeed/F_M_PI), (int)(180.0f * plane_data.yawspeed)/F_M_PI);
|
||||
|
||||
// printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n",
|
||||
// (int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r));
|
||||
|
||||
printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n",
|
||||
(int)(10000 * control_outputs.roll_ailerons), (int)(10000 * control_outputs.pitch_elevator),
|
||||
(int)(10000 * control_outputs.yaw_rudder), (int)(10000 * control_outputs.throttle));
|
||||
(int)(10000.0f * control_outputs.roll_ailerons), (int)(10000.0f * control_outputs.pitch_elevator),
|
||||
(int)(10000.0f * control_outputs.yaw_rudder), (int)(10000.0f * control_outputs.throttle));
|
||||
|
||||
/************************************************************************************************************/
|
||||
}
|
||||
|
|
@ -816,8 +821,8 @@ int fixedwing_control_main(int argc, char *argv[])
|
|||
set_plane_mode();
|
||||
|
||||
/* Calculate the P,Q,R body rates of the aircraft */
|
||||
calc_body_angular_rates(plane_data.roll, plane_data.pitch, plane_data.yaw,
|
||||
plane_data.rollspeed, plane_data.pitchspeed, plane_data.yawspeed);
|
||||
//calc_body_angular_rates(plane_data.roll, plane_data.pitch, plane_data.yaw,
|
||||
// plane_data.rollspeed, plane_data.pitchspeed, plane_data.yawspeed);
|
||||
|
||||
/* Calculate the body frame angles of the aircraft */
|
||||
//calc_bodyframe_angles(plane_data.roll,plane_data.pitch,plane_data.yaw);
|
||||
|
|
@ -830,6 +835,9 @@ int fixedwing_control_main(int argc, char *argv[])
|
|||
|
||||
if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // if we're flying in automated mode
|
||||
|
||||
/*
|
||||
* TAKEOFF hack for HIL
|
||||
*/
|
||||
if (plane_data.mode == TAKEOFF) {
|
||||
control.attitude_control_output[ROLL] = 0;
|
||||
control.attitude_control_output[PITCH] = 5000;
|
||||
|
|
@ -838,9 +846,9 @@ int fixedwing_control_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
if (plane_data.mode == CRUISE) {
|
||||
control.attitude_control_output[ROLL] = (int16_t)(10000 * control_outputs.roll_ailerons);
|
||||
control.attitude_control_output[PITCH] = (int16_t)(10000 * control_outputs.pitch_elevator);
|
||||
control.attitude_control_output[THROTTLE] = (int16_t)(10000 * control_outputs.throttle);
|
||||
control.attitude_control_output[ROLL] = control_outputs.roll_ailerons;
|
||||
control.attitude_control_output[PITCH] = control_outputs.pitch_elevator;
|
||||
control.attitude_control_output[THROTTLE] = control_outputs.throttle;
|
||||
//control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
|
||||
}
|
||||
|
||||
|
|
@ -877,9 +885,9 @@ int fixedwing_control_main(int argc, char *argv[])
|
|||
//control_outputs.yaw_rudder = calc_yaw_rudder(plane_data.hdg);
|
||||
|
||||
} else {
|
||||
control.attitude_control_output[ROLL] = rc.chan[rc.function[ROLL]].scale;
|
||||
control.attitude_control_output[PITCH] = rc.chan[rc.function[PITCH]].scale;
|
||||
control.attitude_control_output[THROTTLE] = rc.chan[rc.function[THROTTLE]].scale;
|
||||
control.attitude_control_output[ROLL] = rc.chan[rc.function[ROLL]].scale/10000;
|
||||
control.attitude_control_output[PITCH] = rc.chan[rc.function[PITCH]].scale/10000;
|
||||
control.attitude_control_output[THROTTLE] = rc.chan[rc.function[THROTTLE]].scale/10000;
|
||||
// since we don't have a yaw rudder
|
||||
//control->attitude_control_output[3] = global_data_rc_channels->chan[YAW].scale;
|
||||
|
||||
|
|
@ -893,9 +901,9 @@ int fixedwing_control_main(int argc, char *argv[])
|
|||
|
||||
/* Servo part */
|
||||
|
||||
buffer_rc[ROLL] = control.attitude_control_output[ROLL];
|
||||
buffer_rc[PITCH] = control.attitude_control_output[PITCH];
|
||||
buffer_rc[THROTTLE] = control.attitude_control_output[THROTTLE];
|
||||
buffer_rc[ROLL] = (int16_t)(10000*control.attitude_control_output[ROLL]);
|
||||
buffer_rc[PITCH] = (int16_t)(10000*control.attitude_control_output[PITCH]);
|
||||
buffer_rc[THROTTLE] = (int16_t)(10000*control.attitude_control_output[THROTTLE]);
|
||||
|
||||
//mix_and_link(mixers, 3, 2, &mixer_buffer);
|
||||
|
||||
|
|
|
|||
|
|
@ -455,7 +455,7 @@ static void *uorb_receiveloop(void *arg)
|
|||
/* struct already globally allocated */
|
||||
/* subscribe to ORB for fixed wing control */
|
||||
int fw_sub = orb_subscribe(ORB_ID(fixedwing_control));
|
||||
orb_set_interval(fw_sub, 200); /* 5 Hz updates */
|
||||
orb_set_interval(fw_sub, 50); /* 20 Hz updates */
|
||||
fds[fdsc_count].fd = fw_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
|
@ -615,6 +615,61 @@ static void *uorb_receiveloop(void *arg)
|
|||
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, fw_control.timestamp / 1000, fw_control.attitude_control_output[0],
|
||||
fw_control.attitude_control_output[1], fw_control.attitude_control_output[2],
|
||||
fw_control.attitude_control_output[3]);
|
||||
|
||||
/* Only send in HIL mode */
|
||||
if (v_status.mode & MAV_MODE_FLAG_HIL_ENABLED) {
|
||||
/* Send the desired attitude from RC or from the autonomous controller */
|
||||
// XXX it should not depend on a RC setting, but on a system_state value
|
||||
|
||||
float roll_ail, pitch_elev, throttle, yaw_rudd;
|
||||
|
||||
if (rc.chan[rc.function[OVERRIDE]].scale < 2000) {
|
||||
|
||||
//orb_copy(ORB_ID(fixedwing_control), fixed_wing_control_sub, &fixed_wing_control);
|
||||
roll_ail = fw_control.attitude_control_output[ROLL];
|
||||
pitch_elev = fw_control.attitude_control_output[PITCH];
|
||||
throttle = fw_control.attitude_control_output[THROTTLE];
|
||||
yaw_rudd = fw_control.attitude_control_output[YAW];
|
||||
|
||||
} else {
|
||||
|
||||
roll_ail = rc.chan[rc.function[ROLL]].scale;
|
||||
pitch_elev = rc.chan[rc.function[PITCH]].scale;
|
||||
throttle = rc.chan[rc.function[THROTTLE]].scale;
|
||||
yaw_rudd = rc.chan[rc.function[YAW]].scale;
|
||||
}
|
||||
|
||||
/* hacked HIL implementation in order for the APM Planner to work
|
||||
* (correct cmd: mavlink_msg_hil_controls_send())
|
||||
*/
|
||||
|
||||
mavlink_msg_rc_channels_scaled_send(chan,
|
||||
hrt_absolute_time(),
|
||||
0, // port 0
|
||||
roll_ail,
|
||||
pitch_elev,
|
||||
throttle,
|
||||
yaw_rudd,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
1 /*rssi=1*/);
|
||||
|
||||
/* correct command duplicate */
|
||||
mavlink_msg_hil_controls_send(chan,
|
||||
hrt_absolute_time(),
|
||||
roll_ail,
|
||||
pitch_elev,
|
||||
yaw_rudd,
|
||||
throttle,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
32, /* HIL_MODE */
|
||||
0);
|
||||
}
|
||||
}
|
||||
|
||||
/* --- VEHICLE GLOBAL POSITION --- */
|
||||
|
|
@ -1144,60 +1199,6 @@ int mavlink_main(int argc, char *argv[])
|
|||
|
||||
lowspeed_counter++;
|
||||
|
||||
/* Only send in HIL mode */
|
||||
if (v_status.mode & MAV_MODE_FLAG_HIL_ENABLED) {
|
||||
/* Send the desired attitude from RC or from the autonomous controller */
|
||||
// XXX it should not depend on a RC setting, but on a system_state value
|
||||
|
||||
float roll_ail, pitch_elev, throttle, yaw_rudd;
|
||||
|
||||
if (rc.chan[rc.function[OVERRIDE]].scale < 2000) {
|
||||
|
||||
//orb_copy(ORB_ID(fixedwing_control), fixed_wing_control_sub, &fixed_wing_control);
|
||||
roll_ail = fw_control.attitude_control_output[ROLL];
|
||||
pitch_elev = fw_control.attitude_control_output[PITCH];
|
||||
throttle = fw_control.attitude_control_output[THROTTLE];
|
||||
yaw_rudd = fw_control.attitude_control_output[YAW];
|
||||
|
||||
} else {
|
||||
|
||||
roll_ail = rc.chan[rc.function[ROLL]].scale;
|
||||
pitch_elev = rc.chan[rc.function[PITCH]].scale;
|
||||
throttle = rc.chan[rc.function[THROTTLE]].scale;
|
||||
yaw_rudd = rc.chan[rc.function[YAW]].scale;
|
||||
}
|
||||
|
||||
/* hacked HIL implementation in order for the APM Planner to work
|
||||
* (correct cmd: mavlink_msg_hil_controls_send())
|
||||
*/
|
||||
|
||||
mavlink_msg_rc_channels_scaled_send(chan,
|
||||
hrt_absolute_time(),
|
||||
0, // port 0
|
||||
roll_ail,
|
||||
pitch_elev,
|
||||
throttle,
|
||||
yaw_rudd,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
1 /*rssi=1*/);
|
||||
/* correct command duplicate */
|
||||
mavlink_msg_hil_controls_send(chan,
|
||||
hrt_absolute_time(),
|
||||
roll_ail,
|
||||
pitch_elev,
|
||||
yaw_rudd,
|
||||
throttle,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
32, /* HIL_MODE */
|
||||
0);
|
||||
}
|
||||
|
||||
/* send parameters at 20 Hz (if queued for sending) */
|
||||
mavlink_pm_queued_send();
|
||||
usleep(50000);
|
||||
|
|
|
|||
|
|
@ -156,87 +156,96 @@ int attitude_estimator_bm_main(int argc, char *argv[])
|
|||
|
||||
unsigned int loopcounter = 0;
|
||||
|
||||
uint64_t last_checkstate_stamp = 0;
|
||||
|
||||
/* Main loop*/
|
||||
while (true) {
|
||||
|
||||
|
||||
/* wait for sensor update */
|
||||
int ret = poll(fds, 1, 5000);
|
||||
int ret = poll(fds, 1, 1000);
|
||||
|
||||
if (ret < 0) {
|
||||
/* XXX this is seriously bad - should be an emergency */
|
||||
} else if (ret == 0) {
|
||||
/* XXX this means no sensor data - should be critical or emergency */
|
||||
printf("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n");
|
||||
continue;
|
||||
}
|
||||
} else {
|
||||
orb_copy(ORB_ID(sensor_combined), sub_raw, &sensor_combined_s_local);
|
||||
|
||||
orb_copy(ORB_ID(sensor_combined), sub_raw, &sensor_combined_s_local);
|
||||
uint64_t now = hrt_absolute_time();
|
||||
unsigned int time_elapsed = now - last_run;
|
||||
last_run = now;
|
||||
|
||||
uint64_t now = hrt_absolute_time();
|
||||
unsigned int time_elapsed = now - last_run;
|
||||
last_run = now;
|
||||
//#if 0
|
||||
|
||||
//#if 0
|
||||
if (time_elapsed > loop_interval_alarm) {
|
||||
//TODO: add warning, cpu overload here
|
||||
if (overloadcounter == 20) {
|
||||
printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
|
||||
overloadcounter = 0;
|
||||
}
|
||||
|
||||
if (time_elapsed > loop_interval_alarm) {
|
||||
//TODO: add warning, cpu overload here
|
||||
if (overloadcounter == 20) {
|
||||
printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
|
||||
overloadcounter = 0;
|
||||
overloadcounter++;
|
||||
}
|
||||
|
||||
overloadcounter++;
|
||||
//#endif
|
||||
// now = hrt_absolute_time();
|
||||
/* filter values */
|
||||
attitude_estimator_bm_update(&sensor_combined_s_local, &euler, &rates, &x_n_b, &y_n_b, &z_n_b);
|
||||
|
||||
// time_elapsed = hrt_absolute_time() - now;
|
||||
// if (blubb == 20)
|
||||
// {
|
||||
// printf("Estimator: %lu\n", time_elapsed);
|
||||
// blubb = 0;
|
||||
// }
|
||||
// blubb++;
|
||||
|
||||
// if (last_data > 0 && sensor_combined_s_local.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", sensor_combined_s_local.timestamp - last_data);
|
||||
|
||||
// printf("%llu -> %llu = %llu\n", last_data, sensor_combined_s_local.timestamp, sensor_combined_s_local.timestamp - last_data);
|
||||
// last_data = sensor_combined_s_local.timestamp;
|
||||
|
||||
/*correct yaw */
|
||||
// euler.z = euler.z + M_PI;
|
||||
|
||||
/* send out */
|
||||
|
||||
att.timestamp = sensor_combined_s_local.timestamp;
|
||||
att.roll = euler.x;
|
||||
att.pitch = euler.y;
|
||||
att.yaw = euler.z + M_PI;
|
||||
|
||||
if (att.yaw > 2.0f * ((float)M_PI)) {
|
||||
att.yaw -= 2.0f * ((float)M_PI);
|
||||
}
|
||||
|
||||
att.rollspeed = rates.x;
|
||||
att.pitchspeed = rates.y;
|
||||
att.yawspeed = rates.z;
|
||||
|
||||
att.R[0][0] = x_n_b.x;
|
||||
att.R[0][1] = x_n_b.y;
|
||||
att.R[0][2] = x_n_b.z;
|
||||
|
||||
// Broadcast
|
||||
if (publishing) orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
|
||||
}
|
||||
|
||||
//#endif
|
||||
// now = hrt_absolute_time();
|
||||
/* filter values */
|
||||
attitude_estimator_bm_update(&sensor_combined_s_local, &euler, &rates, &x_n_b, &y_n_b, &z_n_b);
|
||||
|
||||
// time_elapsed = hrt_absolute_time() - now;
|
||||
// if (blubb == 20)
|
||||
// {
|
||||
// printf("Estimator: %lu\n", time_elapsed);
|
||||
// blubb = 0;
|
||||
// }
|
||||
// blubb++;
|
||||
|
||||
// if (last_data > 0 && sensor_combined_s_local.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", sensor_combined_s_local.timestamp - last_data);
|
||||
|
||||
// printf("%llu -> %llu = %llu\n", last_data, sensor_combined_s_local.timestamp, sensor_combined_s_local.timestamp - last_data);
|
||||
// last_data = sensor_combined_s_local.timestamp;
|
||||
|
||||
/*correct yaw */
|
||||
// euler.z = euler.z + M_PI;
|
||||
|
||||
/* send out */
|
||||
|
||||
att.timestamp = sensor_combined_s_local.timestamp;
|
||||
att.roll = euler.x;
|
||||
att.pitch = euler.y;
|
||||
att.yaw = euler.z + M_PI;
|
||||
|
||||
if (att.yaw > 2.0f * ((float)M_PI)) {
|
||||
att.yaw -= 2.0f * ((float)M_PI);
|
||||
}
|
||||
|
||||
att.rollspeed = rates.x;
|
||||
att.pitchspeed = rates.y;
|
||||
att.yawspeed = rates.z;
|
||||
|
||||
att.R[0][0] = x_n_b.x;
|
||||
att.R[0][1] = x_n_b.y;
|
||||
att.R[0][2] = x_n_b.z;
|
||||
// XXX add remaining entries
|
||||
|
||||
if (loopcounter % 250 == 0) {
|
||||
if (hrt_absolute_time() - last_checkstate_stamp > 500000) {
|
||||
/* Check HIL state */
|
||||
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
|
||||
/* switching from non-HIL to HIL mode */
|
||||
//printf("[attitude_estimator_bm] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
|
||||
if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) {
|
||||
hil_enabled = true;
|
||||
publishing = false;
|
||||
close(pub_att);
|
||||
int ret = close(pub_att);
|
||||
printf("Closing attitude: %i \n", ret);
|
||||
|
||||
/* switching from HIL to non-HIL mode */
|
||||
|
||||
|
|
@ -246,11 +255,9 @@ int attitude_estimator_bm_main(int argc, char *argv[])
|
|||
hil_enabled = false;
|
||||
publishing = true;
|
||||
}
|
||||
last_checkstate_stamp = hrt_absolute_time();
|
||||
}
|
||||
|
||||
// Broadcast
|
||||
if (publishing) orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
|
||||
|
||||
loopcounter++;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -432,10 +432,12 @@ int sensors_main(int argc, char *argv[])
|
|||
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
|
||||
|
||||
/* switching from non-HIL to HIL mode */
|
||||
//printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
|
||||
if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) {
|
||||
hil_enabled = true;
|
||||
publishing = false;
|
||||
close(sensor_pub);
|
||||
int ret = close(sensor_pub);
|
||||
printf("[sensors] Closing sensor pub: %i \n", ret);
|
||||
|
||||
/* switching from HIL to non-HIL mode */
|
||||
|
||||
|
|
|
|||
|
|
@ -62,7 +62,7 @@ struct fixedwing_control_s
|
|||
float setpoint_rate_cast[3];
|
||||
float setpoint_attitude_rate[3];
|
||||
float setpoint_attitude[3];
|
||||
int16_t attitude_control_output[4]; /**< roll, pitch, yaw, throttle */
|
||||
float attitude_control_output[4]; /**< roll, pitch, yaw, throttle */
|
||||
float position_control_output[4];
|
||||
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue