diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-10-10 09:01:11 -0700 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-10-10 09:01:11 -0700 |
commit | 0cb8e782b0e2a6ca0dde4fac641e863c95d04511 (patch) | |
tree | a56b5b9132d16d00341edd7a1f3e8b7f221595a4 /src/modules | |
parent | 55f7c561efde3af1fcf562f71391a94711480988 (diff) | |
parent | 13b07efc49584ea8b864403ff15bae9042ffb3af (diff) | |
download | px4-firmware-0cb8e782b0e2a6ca0dde4fac641e863c95d04511.tar.gz px4-firmware-0cb8e782b0e2a6ca0dde4fac641e863c95d04511.tar.bz2 px4-firmware-0cb8e782b0e2a6ca0dde4fac641e863c95d04511.zip |
Merge pull request #447 from PX4/hil_usb_only
HIL and fixed wing fixes - gives perfect results in simulation
Diffstat (limited to 'src/modules')
20 files changed, 297 insertions, 75 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 01b7b84d0..0c3546b95 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -359,23 +359,35 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t) cmd->param1; uint8_t custom_main_mode = (uint8_t) cmd->param2; + transition_result_t arming_res = TRANSITION_NOT_CHANGED; /* set HIL state */ hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + + /* if HIL got enabled, reset battery status state */ + if (hil_ret == OK && control_mode->flag_system_hil_enabled) { + /* reset the arming mode to disarmed */ + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed); + if (arming_res != TRANSITION_DENIED) { + mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); + } else { + mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); + } + } // TODO remove debug code //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ - transition_result_t arming_res = TRANSITION_NOT_CHANGED; + arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if (safety->safety_switch_available && !safety->safety_off) { + if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; } else { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -385,7 +397,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(status, safety, new_arming_state, armed); + arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); @@ -478,7 +490,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_DENIED; } else { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -532,6 +544,9 @@ static struct actuator_armed_s armed; static struct safety_s safety; +/* flags for control apps */ +struct vehicle_control_mode_s control_mode; + int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -584,10 +599,6 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); - /* flags for control apps */ - struct vehicle_control_mode_s control_mode; - - /* Initialize all flags to false */ memset(&control_mode, 0, sizeof(control_mode)); @@ -876,8 +887,8 @@ int commander_thread_main(int argc, char *argv[]) // warnx("bat v: %2.2f", battery.voltage_v); - /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ - if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) { + /* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */ + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) { status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); @@ -958,9 +969,9 @@ int commander_thread_main(int argc, char *argv[]) battery_tune_played = false; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; @@ -969,6 +980,7 @@ int commander_thread_main(int argc, char *argv[]) critical_voltage_counter++; } else { + low_voltage_counter = 0; critical_voltage_counter = 0; } @@ -978,7 +990,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -1082,7 +1094,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); + res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1104,7 +1116,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed); } stick_on_counter = 0; @@ -1752,7 +1764,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -1792,7 +1804,7 @@ void *commander_low_prio_loop(void *arg) else tune_negative(); - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); break; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 8ce31550f..efe12be57 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -67,7 +67,9 @@ static bool main_state_changed = true; static bool navigation_state_changed = true; transition_result_t -arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed) +arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, + const struct vehicle_control_mode_s *control_mode, + arming_state_t new_arming_state, struct actuator_armed_s *armed) { /* * Perform an atomic state update @@ -82,6 +84,13 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * } else { + /* enforce lockdown in HIL */ + if (control_mode->flag_system_hil_enabled) { + armed->lockdown = true; + } else { + armed->lockdown = false; + } + switch (new_arming_state) { case ARMING_STATE_INIT: @@ -98,7 +107,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow coming from INIT and disarming from ARMED */ if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_ARMED) { + || status->arming_state == ARMING_STATE_ARMED + || control_mode->flag_system_hil_enabled) { /* sensors need to be initialized for STANDBY state */ if (status->condition_system_sensors_initialized) { @@ -115,7 +125,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow arming from STANDBY and IN-AIR-RESTORE */ if ((status->arming_state == ARMING_STATE_STANDBY || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */ + && (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */ ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = true; @@ -466,20 +476,17 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s case HIL_STATE_OFF: - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { - - current_control_mode->flag_system_hil_enabled = false; - mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); - valid_transition = true; - } + /* we're in HIL and unexpected things can happen if we disable HIL now */ + mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)"); + valid_transition = false; break; case HIL_STATE_ON: if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + || current_status->arming_state == ARMING_STATE_STANDBY + || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { current_control_mode->flag_system_hil_enabled = true; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 1641b6f60..0bfdf36a8 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -58,7 +58,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed); + const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 6b98003fd..ae9aaa2da 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -593,6 +593,10 @@ FixedwingAttitudeControl::task_main() pitch_sp = _att_sp.pitch_body; throttle_sp = _att_sp.thrust; + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) + _roll_ctrl.reset_integrator(); + } else { /* * Scale down roll and pitch as the setpoints are radians diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3d5bce134..cd4a0d58e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -74,6 +74,7 @@ #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/navigation_capabilities.h> #include <uORB/topics/parameter_update.h> #include <systemlib/param/param.h> #include <systemlib/err.h> @@ -129,9 +130,11 @@ private: int _accel_sub; /**< body frame accelerations */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ + orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _control_mode; /**< vehicle status */ @@ -312,6 +315,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* publications */ _attitude_sp_pub(-1), + _nav_capabilities_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), @@ -323,6 +327,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _groundspeed_undershoot(0.0f), _global_pos_valid(false) { + _nav_capabilities.turn_distance = 0.0f; + _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); _parameter_handles.l1_damping = param_find("FW_L1_DAMPING"); _parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R"); @@ -625,6 +631,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // else integrators should be constantly reset. if (_control_mode.flag_control_position_enabled) { + /* get circle mode */ + bool was_circle_mode = _l1_control.circle_mode(); + /* execute navigation once we have a setpoint */ if (_setpoint_valid) { @@ -642,7 +651,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { /* - * No valid next waypoint, go for heading hold. + * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ prev_wp.setX(global_triplet.current.lat / 1e7f); @@ -810,6 +819,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } } + if (was_circle_mode && !_l1_control.circle_mode()) { + /* just kicked out of loiter, reset roll integrals */ + _att_sp.roll_reset_integral = true; + } + } else if (0/* seatbelt mode enabled */) { /** SEATBELT FLIGHT **/ @@ -968,6 +982,21 @@ FixedwingPositionControl::task_main() _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } + float turn_distance = _l1_control.switch_distance(_global_triplet.current.turn_distance_xy); + + /* lazily publish navigation capabilities */ + if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { + + /* set new turn distance */ + _nav_capabilities.turn_distance = turn_distance; + + if (_nav_capabilities_pub > 0) { + orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); + } else { + _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); + } + } + } } diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 7a5c2dab2..7c10e297b 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -690,25 +690,25 @@ int mavlink_thread_main(int argc, char *argv[]) lowspeed_counter++; - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); /* check if waypoint has been reached against the last positions */ - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); if (baudrate > 57600) { mavlink_pm_queued_send(); diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index 3175e64ce..e8d707948 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -170,6 +170,28 @@ bool set_special_fields(float param1, float param2, float param3, float param4, sp->param2 = param2; sp->param3 = param3; sp->param4 = param4; + + + /* define the turn distance */ + float orbit = 15.0f; + + if (command == (int)MAV_CMD_NAV_WAYPOINT) { + + orbit = param2; + + } else if (command == (int)MAV_CMD_NAV_LOITER_TURNS || + command == (int)MAV_CMD_NAV_LOITER_TIME || + command == (int)MAV_CMD_NAV_LOITER_UNLIM) { + + orbit = param3; + } else { + + // XXX set default orbit via param + // 15 initialized above + } + + sp->turn_distance_xy = orbit; + sp->turn_distance_z = orbit; } /** @@ -223,10 +245,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, int last_setpoint_index = -1; bool last_setpoint_valid = false; - /* at first waypoint, but cycled once through mission */ - if (index == 0 && last_waypoint_index > 0) { - last_setpoint_index = last_waypoint_index; - } else { + if (index > 0) { last_setpoint_index = index - 1; } @@ -251,10 +270,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, int next_setpoint_index = -1; bool next_setpoint_valid = false; - /* at last waypoint, try to re-loop through mission as default */ - if (index == (wpm->size - 1) && wpm->size > 1) { - next_setpoint_index = 0; - } else if (wpm->size > 1) { + /* next waypoint */ + if (wpm->size > 1) { next_setpoint_index = index + 1; } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index cec2fdc43..0e0ce2723 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -67,6 +67,7 @@ extern bool gcs_link; struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; +struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; @@ -122,6 +123,7 @@ static void l_optical_flow(const struct listener *l); static void l_vehicle_rates_setpoint(const struct listener *l); static void l_home(const struct listener *l); static void l_airspeed(const struct listener *l); +static void l_nav_cap(const struct listener *l); static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -148,6 +150,7 @@ static const struct listener listeners[] = { {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, {l_home, &mavlink_subs.home_sub, 0}, {l_airspeed, &mavlink_subs.airspeed_sub, 0}, + {l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -691,6 +694,19 @@ l_airspeed(const struct listener *l) mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb); } +void +l_nav_cap(const struct listener *l) +{ + + orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap); + + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + hrt_absolute_time() / 1000, + "turn dist", + nav_cap.turn_distance); + +} + static void * uorb_receive_thread(void *arg) { @@ -837,6 +853,11 @@ uorb_receive_start(void) mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ + /* --- NAVIGATION CAPABILITIES --- */ + mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); /* 2Hz updates */ + nav_cap.turn_distance = 0.0f; + /* start the listener loop */ pthread_attr_t uorb_attr; pthread_attr_init(&uorb_attr); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index c5cd0d03e..91773843b 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -67,6 +67,7 @@ #include <uORB/topics/airspeed.h> #include <uORB/topics/battery_status.h> #include <drivers/drv_rc_input.h> +#include <uORB/topics/navigation_capabilities.h> struct mavlink_subscriptions { int sensor_sub; @@ -92,6 +93,7 @@ struct mavlink_subscriptions { int rates_setpoint_sub; int home_sub; int airspeed_sub; + int navigation_capabilities_sub; }; extern struct mavlink_subscriptions mavlink_subs; @@ -102,6 +104,9 @@ extern struct vehicle_global_position_s global_pos; /** Local position */ extern struct vehicle_local_position_s local_pos; +/** navigation capabilities */ +extern struct navigation_capabilities_s nav_cap; + /** Vehicle status */ extern struct vehicle_status_s v_status; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 97472c233..ddad5f0df 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -251,9 +251,8 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) * * The distance calculation is based on the WGS84 geoid (GPS) */ -float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt) +float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z) { - static uint16_t counter; if (seq < wpm->size) { mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); @@ -274,20 +273,21 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float float dxy = radius_earth * c; float dz = alt - wp->z; + *dist_xy = fabsf(dxy); + *dist_z = fabsf(dz); + return sqrtf(dxy * dxy + dz * dz); } else { return -1.0f; } - counter++; - } /* * Calculate distance in local frame (NED) */ -float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z) +float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z) { if (seq < wpm->size) { mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); @@ -296,6 +296,9 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float float dy = (cur->y - y); float dz = (cur->z - z); + *dist_xy = sqrtf(dx * dx + dy * dy); + *dist_z = fabsf(dz); + return sqrtf(dx * dx + dy * dy + dz * dz); } else { @@ -303,11 +306,13 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float } } -void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos) +void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance) { static uint16_t counter; - if (!global_pos->valid && !local_pos->xy_valid) { + if ((!global_pos->valid && !local_pos->xy_valid) || + /* no waypoint */ + wpm->size == 0) { /* nothing to check here, return */ return; } @@ -330,26 +335,37 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ orbit = 15.0f; } + /* keep vertical orbit */ + float vertical_switch_distance = orbit; + + /* Take the larger turn distance - orbit or turn_distance */ + if (orbit < turn_distance) + orbit = turn_distance; + int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt); + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z); } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt); + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z); } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { - dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z); + dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z); } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { /* Check if conditions of mission item are satisfied */ // XXX TODO } - if (dist >= 0.f && dist <= orbit) { + if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { wpm->pos_reached = true; } + // check if required yaw reached float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI); float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw); @@ -463,7 +479,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ } -int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position) +int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap) { /* check for timed-out operations */ if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { @@ -486,7 +502,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio } } - check_waypoints_reached(now, global_position, local_position); + check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance); return OK; } @@ -1009,5 +1025,5 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi } } - check_waypoints_reached(now, global_pos, local_pos); + // check_waypoints_reached(now, global_pos, local_pos); } diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index 96a0ecd30..d7d6b31dc 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -55,6 +55,7 @@ #include <stdbool.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/navigation_capabilities.h> // FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { @@ -120,7 +121,7 @@ typedef struct mavlink_wpm_storage mavlink_wpm_storage; void mavlink_wpm_init(mavlink_wpm_storage *state); int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, - struct vehicle_local_position_s *local_pos); + struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap); void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos); diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 8cb21e54f..9d9ef7c6d 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -674,27 +674,25 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val #ifdef ADC_VSERVO /* PX4IO_P_STATUS_VSERVO */ { - /* - * Coefficients here derived by measurement of the 5-16V - * range on one unit: - * - * XXX pending measurements - * - * slope = xxx - * intercept = xxx - * - * Intercept corrected for best results @ 5.0V. - */ unsigned counts = adc_measure(ADC_VSERVO); if (counts != 0xffff) { - unsigned mV = (4150 + (counts * 46)) / 10 - 200; - unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000; - - r_page_status[PX4IO_P_STATUS_VSERVO] = corrected; + // use 3:1 scaling on 3.3V ADC input + unsigned mV = counts * 9900 / 4096; + r_page_status[PX4IO_P_STATUS_VSERVO] = mV; + } + } +#endif +#ifdef ADC_RSSI + /* PX4IO_P_STATUS_VRSSI */ + { + unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { + // use 1:1 scaling on 3.3V ADC input + unsigned mV = counts * 3300 / 4096; + r_page_status[PX4IO_P_STATUS_VRSSI] = mV; } } #endif - /* XXX PX4IO_P_STATUS_VRSSI */ /* XXX PX4IO_P_STATUS_PRSSI */ SELECT_PAGE(r_page_status); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 085da905c..9ef0d3c83 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1224,6 +1224,9 @@ Sensors::parameter_update_poll(bool forced) void Sensors::adc_poll(struct sensor_combined_s &raw) { + /* only read if publishing */ + if (!_publishing) + return; /* rate limit to 100 Hz */ if (hrt_absolute_time() - _last_adc >= 10000) { diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 3c1e10287..bf84b7945 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -321,6 +321,32 @@ perf_print_counter(perf_counter_t handle) } } +uint64_t +perf_event_count(perf_counter_t handle) +{ + if (handle == NULL) + return 0; + + switch (handle->type) { + case PC_COUNT: + return ((struct perf_ctr_count *)handle)->event_count; + + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + return pce->event_count; + } + + case PC_INTERVAL: { + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + return pci->event_count; + } + + default: + break; + } + return 0; +} + void perf_print_all(void) { diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 4cd8b67a1..e1e3cbe95 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -135,6 +135,14 @@ __EXPORT extern void perf_print_all(void); */ __EXPORT extern void perf_reset_all(void); +/** + * Return current event_count + * + * @param handle The counter returned from perf_alloc. + * @return event_count + */ +__EXPORT extern uint64_t perf_event_count(perf_counter_t handle); + __END_DECLS #endif diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 1eb63a799..3514dca24 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -87,6 +87,9 @@ ORB_DEFINE(safety, struct safety_s); #include "topics/battery_status.h" ORB_DEFINE(battery_status, struct battery_status_s); +#include "topics/servorail_status.h" +ORB_DEFINE(servorail_status, struct servorail_status_s); + #include "topics/vehicle_global_position.h" ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 1ffeda764..e4d2c92ce 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -53,6 +53,7 @@ */ struct differential_pressure_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint64_t error_count; uint16_t differential_pressure_pa; /**< Differential pressure reading */ uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ diff --git a/src/modules/uORB/topics/servorail_status.h b/src/modules/uORB/topics/servorail_status.h new file mode 100644 index 000000000..55668790b --- /dev/null +++ b/src/modules/uORB/topics/servorail_status.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file servorail_status.h + * + * Definition of the servorail status uORB topic. + */ + +#ifndef SERVORAIL_STATUS_H_ +#define SERVORAIL_STATUS_H_ + +#include "../uORB.h" +#include <stdint.h> + +/** + * @addtogroup topics + * @{ + */ + +/** + * Servorail voltages and status + */ +struct servorail_status_s { + uint64_t timestamp; /**< microseconds since system boot */ + float voltage_v; /**< Servo rail voltage in volts */ + float rssi_v; /**< RSSI pin voltage in volts */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(servorail_status); + +#endif diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 686fd765c..1a245132a 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -72,6 +72,8 @@ struct vehicle_attitude_setpoint_s float thrust; /**< Thrust in Newton the power system should generate */ + bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */ + }; /** diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h index 5c8ce1e4d..a56434d3b 100644 --- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h @@ -72,6 +72,8 @@ struct vehicle_global_position_setpoint_s float param2; float param3; float param4; + float turn_distance_xy; /**< The distance on the plane which will mark this as reached */ + float turn_distance_z; /**< The distance in Z direction which will mark this as reached */ }; /** |