diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 05a6292a29..b25e612297 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -204,6 +205,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) memset(&raw, 0, sizeof(raw)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); + struct vehicle_status_s state; + memset(&state, 0, sizeof(state)); uint64_t last_data = 0; uint64_t last_measurement = 0; @@ -216,6 +219,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); + /* subscribe to system state*/ + int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -226,13 +232,15 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) thread_running = true; /* advertise debug value */ - struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; - orb_advert_t pub_dbg = -1; + // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; + // orb_advert_t pub_dbg = -1; + + float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; + // XXX write this out to perf regs /* keep track of sensor updates */ uint32_t sensor_last_count[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; struct attitude_estimator_ekf_params ekf_params; @@ -259,8 +267,12 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) 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 ekf] WARNING: Not getting sensor data - sensor app running?\n"); + /* check if we're in HIL - not getting sensor data is fine then */ + orb_copy(ORB_ID(vehicle_status), sub_state, &state); + if (!state.flag_hil_enabled) { + fprintf(stderr, + "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); + } } else { /* only update parameters if they changed */ @@ -347,9 +359,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) overloadcounter++; } - int32_t z_k_sizes = 9; - // float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; - static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ @@ -392,7 +401,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) continue; } - uint64_t timing_diff = hrt_absolute_time() - timing_start; + // uint64_t timing_diff = hrt_absolute_time() - timing_start; // /* print rotation matrix every 200th time */ if (printcounter % 200 == 0) { diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index dd5463d4e4..bef21848b5 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -211,8 +211,9 @@ HIL::init() if (ret != OK) return ret; - /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + // XXX already claimed with CDEV + ///* try to claim the generic PWM output device node as well - it's OK if we fail at this */ + //ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); if (ret == OK) { log("default PWM output device"); _primary_pwm_device = true; @@ -221,11 +222,11 @@ HIL::init() /* reset GPIOs */ // gpio_reset(); - /* start the IO interface task */ - _task = task_spawn("fmuservo", + /* start the HIL interface task */ + _task = task_spawn("fmuhil", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1024, + 2048, (main_t)&HIL::task_main_trampoline, nullptr); @@ -821,12 +822,12 @@ hil_main(int argc, char *argv[]) if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) { // if (new_mode == PORT1_FULL_PWM || new_mode == PORT1_PWM_AND_GPIO) { // XXX all modes have PWM settings - if (argc > i + 1) { - pwm_update_rate_in_hz = atoi(argv[i + 1]); - } else { - fprintf(stderr, "missing argument for pwm update rate (-u)\n"); - return 1; - } + if (argc > i + 1) { + pwm_update_rate_in_hz = atoi(argv[i + 1]); + } else { + fprintf(stderr, "missing argument for pwm update rate (-u)\n"); + return 1; + } // } else { // fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n"); // } diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index ae888323d8..3eb4a9ef22 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -224,7 +224,7 @@ PX4FMU::init() _task = task_spawn("fmuservo", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1024, + 2048, (main_t)&PX4FMU::task_main_trampoline, nullptr); diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index e1a4e8e8af..3e485274eb 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -218,8 +218,6 @@ handle_message(mavlink_message_t *msg) if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); - //printf("got message\n"); - //printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", mavlink_system.sysid, quad_motors_setpoint.mode); if (mavlink_system.sysid < 4) { @@ -276,61 +274,6 @@ handle_message(mavlink_message_t *msg) /* Publish */ orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); } - - // /* change armed status if required */ - // bool cmd_armed = (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED); - - // bool cmd_generated = false; - - // if (v_status.flag_control_offboard_enabled != cmd_armed) { - // vcmd.param1 = cmd_armed; - // vcmd.param2 = 0; - // vcmd.param3 = 0; - // vcmd.param4 = 0; - // vcmd.param5 = 0; - // vcmd.param6 = 0; - // vcmd.param7 = 0; - // vcmd.command = MAV_CMD_COMPONENT_ARM_DISARM; - // vcmd.target_system = mavlink_system.sysid; - // vcmd.target_component = MAV_COMP_ID_ALL; - // vcmd.source_system = msg->sysid; - // vcmd.source_component = msg->compid; - // vcmd.confirmation = 1; - - // cmd_generated = true; - // } - - // /* check if input has to be enabled */ - // if ((v_status.flag_control_rates_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES)) || - // (v_status.flag_control_attitude_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE)) || - // (v_status.flag_control_velocity_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY)) || - // (v_status.flag_control_position_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION))) { - // vcmd.param1 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES); - // vcmd.param2 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE); - // vcmd.param3 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY); - // vcmd.param4 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION); - // vcmd.param5 = 0; - // vcmd.param6 = 0; - // vcmd.param7 = 0; - // vcmd.command = PX4_CMD_CONTROLLER_SELECTION; - // vcmd.target_system = mavlink_system.sysid; - // vcmd.target_component = MAV_COMP_ID_ALL; - // vcmd.source_system = msg->sysid; - // vcmd.source_component = msg->compid; - // vcmd.confirmation = 1; - - // cmd_generated = true; - // } - - // if (cmd_generated) { - // /* check if topic is advertised */ - // if (cmd_pub <= 0) { - // cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - // } else { - // /* create command */ - // orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); - // } - // } } } @@ -342,8 +285,6 @@ handle_message(mavlink_message_t *msg) * COMMAND_LONG message or a SET_MODE message */ - // printf("\n HIL ENABLED?: %s \n",(mavlink_hil_enabled)?"true":"false"); - if (mavlink_hil_enabled) { if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) { @@ -351,20 +292,6 @@ handle_message(mavlink_message_t *msg) mavlink_hil_state_t hil_state; mavlink_msg_hil_state_decode(msg, &hil_state); - // printf("\n HILSTATE : \n LAT: %i \n LON: %i \n ALT: %i \n " - // "ROLL %i \n PITCH %i \n YAW %i \n" - // "ROLLSPEED: %i \n PITCHSPEED: %i \n, YAWSPEED: %i \n", - // hil_state.lat/1000000, // 1e7 - // hil_state.lon/1000000, // 1e7 - // hil_state.alt/1000, // mm - // hil_state.roll, // float rad - // hil_state.pitch, // float rad - // hil_state.yaw, // float rad - // hil_state.rollspeed, // float rad/s - // hil_state.pitchspeed, // float rad/s - // hil_state.yawspeed); // float rad/s - - hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f;