From 802ce6ae0b865e9b9637c4251917090d3acabee1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Dec 2015 20:29:22 +0100 Subject: [PATCH 01/16] Always set home position on takeoff --- src/modules/commander/commander.cpp | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1f5453aed5..b968a094a3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -392,9 +392,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "arm")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); - if (TRANSITION_CHANGED == arm_disarm(true, mavlink_fd_local, "command line")) { - warnx("note: not updating home position on commandline arming!"); - } else { + if (TRANSITION_CHANGED != arm_disarm(true, mavlink_fd_local, "command line")) { warnx("arming failed"); } px4_close(mavlink_fd_local); @@ -412,11 +410,12 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "takeoff")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); - if (TRANSITION_CHANGED == arm_disarm(true, mavlink_fd_local, "command line")) { - warnx("note: not updating home position on commandline arming!"); - /* see if we got a home position */ - if (status.condition_home_position_valid) { + /* see if we got a home position */ + if (status.condition_home_position_valid) { + + if (TRANSITION_CHANGED == arm_disarm(true, mavlink_fd_local, "command line")) { + vehicle_command_s cmd = {}; cmd.target_system = status.system_id; cmd.target_component = status.component_id; @@ -433,12 +432,13 @@ int commander_main(int argc, char *argv[]) (void)orb_advertise(ORB_ID(vehicle_command), &cmd); } else { - warnx("rejecting takeoff, no position lock yet"); + warnx("arming failed"); } } else { - warnx("arming failed"); + warnx("rejecting takeoff, no position lock yet. Please retry.."); } + px4_close(mavlink_fd_local); return 0; } @@ -707,6 +707,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } else { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ + if (cmd_arms && (arming_res == TRANSITION_CHANGED) && + (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) { + + commander_set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude); + } } } } From 2041e3f3bcce14dd9db679b96a69eb61c5958d8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 09:19:39 +0100 Subject: [PATCH 02/16] Handle unsupported commands gracefully in terms of their numerical range. Introduce an error code for out-of-range commands. --- src/modules/mavlink/mavlink_mission.cpp | 9 ++++++++- src/modules/navigator/navigation.h | 4 ++-- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index bb0908908c..1c99d1d583 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -813,7 +813,14 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ - mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + + /* unsigned, so can't be negative, only check for overflow */ + if (mavlink_mission_item->command >= NAV_CMD_INVALID) { + mission_item->nav_cmd = NAV_CMD_INVALID; + + } else { + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + } mission_item->autocontinue = mavlink_mission_item->autocontinue; // mission_item->index = mavlink_mission_item->seq; diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index b0fabc08f5..89f77b4752 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -63,8 +63,8 @@ enum NAV_CMD { NAV_CMD_PATHPLANNING = 81, NAV_CMD_DO_JUMP = 177, NAV_CMD_DO_SET_SERVO=183, - NAV_CMD_DO_REPEAT_SERVO=184 - + NAV_CMD_DO_REPEAT_SERVO=184, + NAV_CMD_INVALID=UINT16_MAX /* ensure that casting a large number results in a specific error */ }; enum ORIGIN { From e525ab3b73b449f2c4d7c477e1233c706c57b5a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 13:44:51 +0100 Subject: [PATCH 03/16] Commander: Fix accel cal duration --- .../commander/accelerometer_calibration.cpp | 32 ++++++++++++------- 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index d7a58e6254..dd6d5e3a9d 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -326,7 +326,7 @@ int do_accel_calibration(int mavlink_fd) static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) { - const unsigned samples_num = 3000; + const unsigned samples_num = 750; accel_worker_data_t* worker_data = (accel_worker_data_t*)(data); mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation)); @@ -574,7 +574,8 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref int do_level_calibration(int mavlink_fd) { const unsigned cal_time = 5; const unsigned cal_hz = 100; - const unsigned settle_time = 30; + unsigned settle_time = 30; + bool success = false; int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); struct vehicle_attitude_s att; @@ -582,18 +583,25 @@ int do_level_calibration(int mavlink_fd) { mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "level"); - param_t roll_offset_handler = param_find("SENS_BOARD_X_OFF"); - param_t pitch_offset_handler = param_find("SENS_BOARD_Y_OFF"); + param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF"); + param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF"); + param_t board_rot_handle = param_find("SENS_BOARD_ROT"); // save old values if calibration fails float roll_offset_current; float pitch_offset_current; - param_get(roll_offset_handler, &roll_offset_current); - param_get(pitch_offset_handler, &pitch_offset_current); + int32_t board_rot_current = 0; + param_get(roll_offset_handle, &roll_offset_current); + param_get(pitch_offset_handle, &pitch_offset_current); + param_get(board_rot_handle, &board_rot_current); + + if (board_rot_current == 0) { + settle_time = 0; + } float zero = 0.0f; - param_set(roll_offset_handler, &zero); - param_set(pitch_offset_handler, &zero); + param_set(roll_offset_handle, &zero); + param_set(pitch_offset_handle, &zero); px4_pollfd_struct_t fds[1]; @@ -647,8 +655,8 @@ int do_level_calibration(int mavlink_fd) { } else { roll_mean *= (float)M_RAD_TO_DEG; pitch_mean *= (float)M_RAD_TO_DEG; - param_set(roll_offset_handler, &roll_mean); - param_set(pitch_offset_handler, &pitch_mean); + param_set(roll_offset_handle, &roll_mean); + param_set(pitch_offset_handle, &pitch_mean); success = true; } @@ -658,8 +666,8 @@ out: return 0; } else { // set old parameters - param_set(roll_offset_handler, &roll_offset_current); - param_set(pitch_offset_handler, &pitch_offset_current); + param_set(roll_offset_handle, &roll_offset_current); + param_set(pitch_offset_handle, &pitch_offset_current); mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level"); return 1; } From e9b4946d9f32943b6b59afef8593bd2c25a8cd2d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 14:14:17 +0100 Subject: [PATCH 04/16] Only update heartbeat timestamp if current --- src/modules/commander/commander.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b968a094a3..cccd2bec0c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1556,7 +1556,9 @@ int commander_thread_main(int argc, char *argv[]) _usb_telemetry_active = true; } - telemetry_last_heartbeat[i] = telemetry.heartbeat_time; + if (telemetry.heartbeat_time > 0) { + telemetry_last_heartbeat[i] = telemetry.heartbeat_time; + } } } From 5d9048280fc19c6663e3df21c516daa847159e0c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 14:15:24 +0100 Subject: [PATCH 05/16] Fix code style --- src/modules/mavlink/mavlink_mission.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 1c99d1d583..746689dd52 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -813,8 +813,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ - - /* unsigned, so can't be negative, only check for overflow */ + + /* unsigned, so can't be negative, only check for overflow */ if (mavlink_mission_item->command >= NAV_CMD_INVALID) { mission_item->nav_cmd = NAV_CMD_INVALID; From 43aa6f64d9f2437ce244533f0c0f72f6e5002d59 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 14:18:38 +0100 Subject: [PATCH 06/16] MAVLink: Use correct multi-instance syntax --- src/modules/mavlink/mavlink_receiver.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f4a3fbd208..4d50a93364 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1178,7 +1178,8 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) tstatus.heartbeat_time = tstatus.timestamp; if (_telemetry_status_pub == nullptr) { - _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, NULL, ORB_PRIO_HIGH); + int multi_instance; + _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, multi_instance, ORB_PRIO_HIGH); } else { orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); From 6a567cd9116d5c149a6b7e1fe4d1e2b2081384cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 14:43:09 +0100 Subject: [PATCH 07/16] Fix MAVLink compile error --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4d50a93364..51f08bb01c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1179,7 +1179,7 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) if (_telemetry_status_pub == nullptr) { int multi_instance; - _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, multi_instance, ORB_PRIO_HIGH); + _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH); } else { orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); From e1597c2aa965b1ef0db294f7f2ffb15eedf8112f Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 12 Dec 2015 10:47:51 +0100 Subject: [PATCH 08/16] vtol standard: add weight to mc motor thrust to avoid sudden changes in rmp after transitions --- src/modules/vtol_att_control/standard.cpp | 11 ++++++++++- src/modules/vtol_att_control/tailsitter.cpp | 5 +++-- src/modules/vtol_att_control/vtol_type.h | 2 ++ 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 8a5896999e..c5a015eb4d 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -54,6 +54,7 @@ Standard::Standard(VtolAttitudeControl *attc) : _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + _mc_throttle_weight = 1.0f; _params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR"); _params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR"); @@ -113,6 +114,7 @@ void Standard::update_vtol_state() _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + _mc_throttle_weight = 1.0f; } else if (_vtol_schedule.flight_mode == FW_MODE) { // transition to mc mode @@ -126,6 +128,7 @@ void Standard::update_vtol_state() _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + _mc_throttle_weight = 1.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // transition to MC mode if transition time has passed @@ -151,6 +154,7 @@ void Standard::update_vtol_state() _mc_roll_weight = 0.0f; _mc_pitch_weight = 0.0f; _mc_yaw_weight = 0.0f; + _mc_throttle_weight = 0.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode @@ -207,12 +211,14 @@ void Standard::update_transition_state() _mc_roll_weight = weight; _mc_pitch_weight = weight; _mc_yaw_weight = weight; + _mc_throttle_weight = weight; } else { // at low speeds give full weight to mc _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + _mc_throttle_weight = 1.0f; } } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { @@ -223,11 +229,13 @@ void Standard::update_transition_state() _mc_roll_weight = weight; _mc_pitch_weight = weight; _mc_yaw_weight = weight; + _mc_throttle_weight = weight; } else { _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; + _mc_throttle_weight = 1.0f; } // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again @@ -241,6 +249,7 @@ void Standard::update_transition_state() _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); _mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f); _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); + _mc_throttle_weight = math::constrain(_mc_throttle_weight, 0.0f, 1.0f); } void Standard::update_mc_state() @@ -280,7 +289,7 @@ void Standard::fill_actuator_outputs() _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; // yaw _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle + _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight; // throttle /* fixed wing controls */ _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 106f91a8e4..e7daa2c3a3 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -178,7 +178,7 @@ void Tailsitter::update_vtol_state() // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans - && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) || !_armed->armed) { + && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) || !_armed->armed) { _vtol_schedule.flight_mode = FW_MODE; //_vtol_schedule.transition_start = hrt_absolute_time(); } @@ -461,7 +461,8 @@ void Tailsitter::fill_actuator_outputs() // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon } break; diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 196751016c..2f9211885b 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -117,6 +117,8 @@ protected: float _mc_roll_weight; // weight for multicopter attitude controller roll output float _mc_pitch_weight; // weight for multicopter attitude controller pitch output float _mc_yaw_weight; // weight for multicopter attitude controller yaw output + float _mc_throttle_weight; // weight for multicopter throttle command. Used to avoid + // motors spinning up or cutting too fast whend doing transitions. float _yaw_transition; // yaw angle in which transition will take place float _pitch_transition_start; // pitch angle at the start of transition (tailsitter) From 23e9693641d1796adffca04c33518dc366dcea1f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Dec 2015 09:48:11 +0100 Subject: [PATCH 09/16] Allow chrooting the application --- Tools/sitl_run.sh | 15 ++++++++++++--- src/platforms/posix/main.cpp | 35 ++++++++++++++++++++++++++++++++++- 2 files changed, 46 insertions(+), 4 deletions(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index dcfd21930b..a30897fe7a 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -14,6 +14,15 @@ echo program: $program echo model: $model echo build_path: $build_path +if [ "$chroot" == "1" ] +then + chroot_enabled=-c + sudo_enabled=sudo +else + chroot_enabled="" + sudo_enabled="" +fi + if [ "$model" == "" ] || [ "$model" == "none" ] then echo "empty model, setting iris as default" @@ -48,7 +57,7 @@ if [ "$program" == "jmavsim" ] && [ "$no_sim" == "" ] then cd Tools/jMAVSim ant - nice -n -10 java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 & + java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 & SIM_PID=`echo $!` elif [ "$program" == "gazebo" ] && [ "$no_sim" == "" ] then @@ -65,7 +74,7 @@ then cd Tools/sitl_gazebo/Build cmake -Wno-dev .. make -j4 - nice -n -10 gzserver --verbose ../worlds/${model}.world & + gzserver --verbose ../worlds/${model}.world & SIM_PID=`echo $!` gzclient --verbose & GUI_PID=`echo $!` @@ -96,7 +105,7 @@ elif [ "$debugger" == "valgrind" ] then valgrind ./mainapp ../../../../${rc_script}_${program}_${model} else - nice -n -10 ./mainapp ../../../../${rc_script}_${program}_${model} + $sudo_enabled ./mainapp $chroot_enabled ../../../../${rc_script}_${program}_${model} fi if [ "$program" == "jmavsim" ] diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 7bf431b4f4..24ba4f6e66 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include "apps.h" #include "px4_middleware.h" #include "DriverFramework.hpp" @@ -157,6 +158,7 @@ static void process_line(string &line, bool exit_on_fail) int main(int argc, char **argv) { bool daemon_mode = false; + bool chroot_on = false; signal(SIGINT, _SigIntHandler); signal(SIGFPE, _SigFpeHandler); @@ -174,6 +176,9 @@ int main(int argc, char **argv) usage(); return 0; + } else if (strcmp(argv[index], "-c") == 0) { + chroot_on = true; + } else { PX4_WARN("Unknown/unhandled parameter: %s", argv[index]); return 1; @@ -203,7 +208,7 @@ int main(int argc, char **argv) px4::init(argc, argv, "mainapp"); - //if commandfile is present, process the commands from the file + // if commandfile is present, process the commands from the file if (commands_file != nullptr) { ifstream infile(commands_file); @@ -217,6 +222,34 @@ int main(int argc, char **argv) } } + if (chroot_on) { + // Lock this application in the current working dir + // this is not an attempt to secure the environment, + // rather, to replicate a deployed file system. + char pwd_path[PATH_MAX]; + const char *folderpath = "/rootfs/"; + + if (nullptr == getcwd(pwd_path, sizeof(pwd_path))) { + PX4_ERR("Failed aquiring working dir, abort."); + exit(1); + } + + if (nullptr == strcat(pwd_path, folderpath)) { + PX4_ERR("Failed completing path, abort."); + exit(1); + } + + if (chroot(pwd_path)) { + PX4_ERR("Failed chrooting application, path: %s, error: %s.", pwd_path, strerror(errno)); + exit(1); + } + + if (chdir("/")) { + PX4_ERR("Failed changing to root dir, path: %s, error: %s.", pwd_path, strerror(errno)); + exit(1); + } + } + if (!daemon_mode) { string mystr = ""; string string_buffer[CMD_BUFF_SIZE]; From e18c497513f3ed5cb26fb6d6d85b69e71104e41c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Dec 2015 10:18:32 +0100 Subject: [PATCH 10/16] POSIX: Add missing header --- src/platforms/posix/main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 24ba4f6e66..da0aed8d8c 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include "apps.h" #include "px4_middleware.h" #include "DriverFramework.hpp" From d0735eae25c9943f5235efade4b42c4f12b0c315 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Dec 2015 13:28:42 +0100 Subject: [PATCH 11/16] Fix SITL Linux compile error --- src/platforms/posix/main.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index da0aed8d8c..4d6663006d 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -227,7 +227,14 @@ int main(int argc, char **argv) // Lock this application in the current working dir // this is not an attempt to secure the environment, // rather, to replicate a deployed file system. - char pwd_path[PATH_MAX]; + +#ifdef PATH_MAX + const unsigned path_max_len = PATH_MAX; +#else + const unsigned path_max_len = 1024; +#endif + + char pwd_path[path_max_len]; const char *folderpath = "/rootfs/"; if (nullptr == getcwd(pwd_path, sizeof(pwd_path))) { From 3c8411d7edc2b6a7b983bf43c4b9fae015fa509a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Dec 2015 13:28:58 +0100 Subject: [PATCH 12/16] Navigator: Fix return value typo for abs altitude --- src/modules/navigator/mission.cpp | 2 +- src/modules/navigator/mission.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index e4068cac38..0a13706c87 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -303,7 +303,7 @@ Mission::advance_mission() } } -int +float Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item) { if (_mission_item.altitude_is_relative) { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index f367da1c7d..b0117cf1fe 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -133,7 +133,7 @@ private: */ void altitude_sp_foh_reset(); - int get_absolute_altitude_for_item(struct mission_item_s &mission_item); + float get_absolute_altitude_for_item(struct mission_item_s &mission_item); /** * Read current or next mission item from the dataman and watch out for DO_JUMPS From 745dc13d67a84be8cd5491910b82c8a6d12c04fd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Dec 2015 09:12:16 +0100 Subject: [PATCH 13/16] Add simple example app --- cmake/configs/nuttx_px4fmu-v4_default.cmake | 2 +- cmake/configs/posix_sitl_default.cmake | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index a6dd256a34..5c9de8fd56 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -143,7 +143,7 @@ set(config_module_list #examples/math_demo # Tutorial code from # https://px4.io/dev/px4_simple_app - #examples/px4_simple_app + examples/px4_simple_app # Tutorial code from # https://px4.io/dev/daemon diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index b3c9205586..4f4be42ab2 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -59,6 +59,7 @@ set(config_module_list lib/launchdetection lib/terrain_estimation lib/runway_takeoff + examples/px4_simple_app ) set(config_extra_builtin_cmds From 8b76d5e941796860ce3777263c1244008f8e9ee5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Dec 2015 09:12:29 +0100 Subject: [PATCH 14/16] Update simple app example to use multiplatform API --- src/examples/px4_simple_app/px4_simple_app.c | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index ea54574bb9..6007d1fa44 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -39,6 +39,8 @@ */ #include +#include +#include #include #include #include @@ -52,7 +54,7 @@ __EXPORT int px4_simple_app_main(int argc, char *argv[]); int px4_simple_app_main(int argc, char *argv[]) { - printf("Hello Sky!\n"); + PX4_INFO("Hello Sky!"); /* subscribe to sensor_combined topic */ int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); @@ -64,7 +66,7 @@ int px4_simple_app_main(int argc, char *argv[]) orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); /* one could wait for multiple topics with this technique, just using one here */ - struct pollfd fds[] = { + px4_pollfd_struct_t fds[] = { { .fd = sensor_sub_fd, .events = POLLIN }, /* there could be more file descriptors here, in the form like: * { .fd = other_sub_fd, .events = POLLIN }, @@ -75,18 +77,18 @@ int px4_simple_app_main(int argc, char *argv[]) for (int i = 0; i < 5; i++) { /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */ - int poll_ret = poll(fds, 1, 1000); + int poll_ret = px4_poll(fds, 1, 1000); /* handle the poll result */ if (poll_ret == 0) { /* this means none of our providers is giving us data */ - printf("[px4_simple_app] Got no data within a second\n"); + PX4_ERR("[px4_simple_app] Got no data within a second"); } else if (poll_ret < 0) { /* this is seriously bad - should be an emergency */ if (error_counter < 10 || error_counter % 50 == 0) { /* use a counter to prevent flooding (and slowing us down) */ - printf("[px4_simple_app] ERROR return value from poll(): %d\n" + PX4_ERR("[px4_simple_app] ERROR return value from poll(): %d" , poll_ret); } @@ -99,7 +101,7 @@ int px4_simple_app_main(int argc, char *argv[]) struct sensor_combined_s raw; /* copy sensors raw data into local buffer */ orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw); - printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n", + PX4_WARN("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f", (double)raw.accelerometer_m_s2[0], (double)raw.accelerometer_m_s2[1], (double)raw.accelerometer_m_s2[2]); @@ -116,6 +118,7 @@ int px4_simple_app_main(int argc, char *argv[]) */ } } + PX4_INFO("exiting"); return 0; } From 977aee9f62a46a3c0ead2b1f04ffe4fd6d4b78e9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Dec 2015 09:38:21 +0100 Subject: [PATCH 15/16] Simple app: fix code style --- src/examples/px4_simple_app/px4_simple_app.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 6007d1fa44..23cd124130 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -89,7 +89,7 @@ int px4_simple_app_main(int argc, char *argv[]) if (error_counter < 10 || error_counter % 50 == 0) { /* use a counter to prevent flooding (and slowing us down) */ PX4_ERR("[px4_simple_app] ERROR return value from poll(): %d" - , poll_ret); + , poll_ret); } error_counter++; @@ -102,9 +102,9 @@ int px4_simple_app_main(int argc, char *argv[]) /* copy sensors raw data into local buffer */ orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw); PX4_WARN("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f", - (double)raw.accelerometer_m_s2[0], - (double)raw.accelerometer_m_s2[1], - (double)raw.accelerometer_m_s2[2]); + (double)raw.accelerometer_m_s2[0], + (double)raw.accelerometer_m_s2[1], + (double)raw.accelerometer_m_s2[2]); /* set att and publish this information for other apps */ att.roll = raw.accelerometer_m_s2[0]; @@ -118,6 +118,7 @@ int px4_simple_app_main(int argc, char *argv[]) */ } } + PX4_INFO("exiting"); return 0; From 63da175b3fbd9f912b3332df01b628363a29e238 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Dec 2015 12:47:37 +0100 Subject: [PATCH 16/16] Fix spurious mission warning --- src/modules/navigator/mission.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 0a13706c87..299b69b3d0 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -656,8 +656,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s for (int i = 0; i < 10; i++) { if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { - /* mission item index out of bounds */ - mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count); + /* mission item index out of bounds - if they are equal, we just reached the end */ + if (*mission_index_ptr != (int)mission->count) { + mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count); + } return false; }