diff options
author | Ivan Ovinnikov <oivan@ethz.ch> | 2012-08-07 14:18:09 +0200 |
---|---|---|
committer | Ivan Ovinnikov <oivan@ethz.ch> | 2012-08-07 14:18:09 +0200 |
commit | 9536bfa3ca239e310be033e8d83a7cc293ebfff6 (patch) | |
tree | c4681da8a3d66f7993ae4cebb887ba3103e53029 | |
parent | 2b09a7914f186831e5a4fca6133355f8aadbe428 (diff) | |
download | px4-firmware-9536bfa3ca239e310be033e8d83a7cc293ebfff6.tar.gz px4-firmware-9536bfa3ca239e310be033e8d83a7cc293ebfff6.tar.bz2 px4-firmware-9536bfa3ca239e310be033e8d83a7cc293ebfff6.zip |
HIL fixed, fixedwing control fixes
-rw-r--r-- | apps/commander/state_machine_helper.c | 3 | ||||
-rw-r--r-- | apps/fixedwing_control/fixedwing_control.c | 96 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 111 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_estimator_bm.c | 105 | ||||
-rw-r--r-- | apps/sensors/sensors.c | 4 | ||||
-rw-r--r-- | apps/uORB/topics/fixedwing_control.h | 2 |
6 files changed, 171 insertions, 150 deletions
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 91eea2117..a71bb797d 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -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 */ diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index 9faa257ca..bad62edcf 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -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); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index d9dd09669..74f7b2099 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -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); diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index 8551b5e1c..af7ad7187 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -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); -//#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++; -// 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); -// 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; -// 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; - /*correct yaw */ -// euler.z = euler.z + M_PI; + /* send out */ - /* send out */ + att.timestamp = sensor_combined_s_local.timestamp; + att.roll = euler.x; + att.pitch = euler.y; + att.yaw = euler.z + M_PI; - 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; - if (att.yaw > 2.0f * ((float)M_PI)) { - att.yaw -= 2.0f * ((float)M_PI); + 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); } - 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++; } diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index 45e7687fd..461b9805d 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -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 */ diff --git a/apps/uORB/topics/fixedwing_control.h b/apps/uORB/topics/fixedwing_control.h index e904709b6..5f70cad40 100644 --- a/apps/uORB/topics/fixedwing_control.h +++ b/apps/uORB/topics/fixedwing_control.h @@ -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]; }; |