From d84639f8c45cc402c1ab504f832beee5d6cd8153 Mon Sep 17 00:00:00 2001
From: jwilson <jwilson@qti.qualcomm.com>
Date: Fri, 12 Feb 2016 21:39:50 -0800
Subject: [PATCH] This code is flyable, but a few problems exist which cause
values in the .config files to be overwritten by the defaults in the .XML
file.
---
.../eagle/200qx/px4-flight-v1-board.config | 80 +++++++++++++++++++
...ight.config => px4-flight-v2-board.config} | 2 +-
posix-configs/eagle/210qc/px4-flight.config | 1 +
src/modules/commander/commander.cpp | 69 ++++++++++++----
.../mc_att_control/mc_att_control_main.cpp | 16 ++++
src/modules/sensors/sensors.cpp | 13 ++-
src/platforms/qurt/px4_layer/main.cpp | 6 ++
7 files changed, 171 insertions(+), 16 deletions(-)
create mode 100644 posix-configs/eagle/200qx/px4-flight-v1-board.config
rename posix-configs/eagle/200qx/{px4-flight.config => px4-flight-v2-board.config} (98%)
diff --git a/posix-configs/eagle/200qx/px4-flight-v1-board.config b/posix-configs/eagle/200qx/px4-flight-v1-board.config
new file mode 100644
index 0000000000..d7f54064bd
--- /dev/null
+++ b/posix-configs/eagle/200qx/px4-flight-v1-board.config
@@ -0,0 +1,80 @@
+uorb start
+param set CAL_GYRO0_ID 2293760
+param set CAL_ACC0_ID 1310720
+param set CAL_ACC1_ID 1376256
+param set CAL_MAG0_ID 196608
+commander start
+param set RC_RECEIVER_TYPE 1
+rc_receiver start -D /dev/tty-1
+attitude_estimator_q start
+position_estimator_inav start
+mc_pos_control start
+mc_att_control start
+sleep 1
+param set SYS_AUTOSTART 4001
+param set SYS_AUTOCONFIG 1
+param set MAV_TYPE 2
+param set RC_MAP_THROTTLE 1
+param set RC_MAP_ROLL 2
+param set RC_MAP_PITCH 3
+param set RC_MAP_YAW 4
+param set RC_MAP_MODE_SW 5
+param set RC_MAP_POSCTL_SW 6
+param set RC1_MAX 1900
+param set RC1_MIN 1099
+param set RC1_TRIM 1099
+param set RC1_REV 1
+param set RC2_MAX 1900
+param set RC2_MIN 1099
+param set RC2_TRIM 1500
+param set RC2_REV -1
+param set RC3_MAX 1900
+param set RC3_MIN 1099
+param set RC3_TRIM 1500
+param set RC3_REV 1
+param set RC4_MAX 1900
+param set RC4_MIN 1099
+param set RC4_TRIM 1500
+param set RC4_REV -1
+param set RC5_MAX 1900
+param set RC5_MIN 1099
+param set RC5_TRIM 1500
+param set RC5_REV 1
+param set RC6_MAX 1900
+param set RC6_MIN 1099
+param set RC6_TRIM 1099
+param set RC6_REV 1
+sensors start
+param set MC_YAW_P 7.0
+param set MC_YAWRATE_P 0.08
+param set MC_YAWRATE_I 0.0
+param set MC_YAWRATE_D 0
+param set MC_PITCH_P 7.0
+param set MC_PITCHRATE_P 0.08
+param set MC_PITCHRATE_I 0.0
+param set MC_PITCHRATE_D 0.0001
+param set MC_ROLL_P 7.0
+param set MC_ROLLRATE_P 0.08
+param set MC_ROLLRATE_I 0.0
+param set MC_ROLLRATE_D 0.0001
+param set ATT_W_MAG 0.00
+param set PE_MAG_NOISE 1.0f
+param set SENS_BOARD_ROT 6
+param set MPU_GYRO_LPF_ENUM 4
+param set MPU_ACC_LPF_ENUM 4
+param set MPU_SAMPLE_RATE_ENUM 2
+sleep 1
+mpu9x50 start -D /dev/spi-1
+param set UART_ESC_MODEL 0
+param set UART_ESC_BAUDRATE 250000
+param set UART_ESC_PX4MOTOR1 2
+param set UART_ESC_PX4MOTOR2 4
+param set UART_ESC_PX4MOTOR3 1
+param set UART_ESC_PX4MOTOR4 3
+sleep 1
+uart_esc start -D /dev/tty-2
+csr_gps start -D /dev/tty-3
+list_devices
+list_files
+list_tasks
+list_topics
diff --git a/posix-configs/eagle/200qx/px4-flight.config b/posix-configs/eagle/200qx/px4-flight-v2-board.config
similarity index 98%
rename from posix-configs/eagle/200qx/px4-flight.config
rename to posix-configs/eagle/200qx/px4-flight-v2-board.config
index 814f375ae8..7e323c2ecd 100644
--- a/posix-configs/eagle/200qx/px4-flight.config
+++ b/posix-configs/eagle/200qx/px4-flight-v2-board.config
@@ -13,7 +13,7 @@ mc_att_control start
sleep 1
param set SYS_AUTOSTART 4001
param set SYS_AUTOCONFIG 1
-param set MAV_TYPE 2​
+param set MAV_TYPE 2
param set RC_MAP_THROTTLE 1
param set RC_MAP_ROLL 2
param set RC_MAP_PITCH 3
diff --git a/posix-configs/eagle/210qc/px4-flight.config b/posix-configs/eagle/210qc/px4-flight.config
index 4d152f593d..d1f207d513 100644
--- a/posix-configs/eagle/210qc/px4-flight.config
+++ b/posix-configs/eagle/210qc/px4-flight.config
@@ -3,6 +3,7 @@ param set CAL_GYRO0_ID 2293760
param set CAL_ACC0_ID 1310720
param set CAL_ACC1_ID 1376256
param set CAL_MAG0_ID 196608
+param set MAV_TYPE 2
commander start
param set RC_RECEIVER_TYPE 1
rc_receiver start -D /dev/tty-1
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index eb5210259e..821f6d8d3d 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -724,10 +724,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
// Refuse to arm if preflight checks have failed
if ((!status.hil_state) != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed.");
- cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
+ cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
}
-
+
}
transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command");
@@ -1155,7 +1155,7 @@ int commander_thread_main(int argc, char *argv[])
// XXX for now just set sensors as initialized
status.condition_system_sensors_initialized = true;
-
+
status.condition_system_prearm_error_reported = false;
status.condition_system_hotplug_timeout = false;
@@ -2196,7 +2196,11 @@ int commander_thread_main(int argc, char *argv[])
if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
(hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f))) {
#else
- // XXX HACK: remove old data check due to timestamp issue in QURT
+ // TODO-JYW: TESTING-TESTING
+ warnx("status.rc_input_blocked: %d, sp_man.timestamp: %d", status.rc_input_blocked, sp_man.timestamp);
+ // TODO-JYW: TESTING-TESTING
+
+ // HACK: remove old data check due to timestamp issue in QURT
if (!status.rc_input_blocked && sp_man.timestamp != 0) {
#endif
/* handle the case where RC signal was regained */
@@ -2214,6 +2218,14 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_lost = false;
+ // TODO-JYW: TESTING-TESTING
+ warnx("lower left stick: status.is_rotary_wing: %d, status.rc_input_mode: %d, status.arming_state: %d",
+ status.is_rotary_wing, status.rc_input_mode, status.arming_state, status.main_state, status.condition_landed, (double)sp_man.r, (double)sp_man.z);
+ warnx("lower left stick: status.is_rotary_wing: %d, status.rc_input_mode: %d, status.arming_state: %d, status.main_state: %d, status.condition_landed: %d, sp_man.r: %f, sp_man.z: %f",
+ status.is_rotary_wing, status.rc_input_mode, status.arming_state, status.main_state, status.condition_landed, (double)sp_man.r, (double)sp_man.z);
+ // TODO-JYW: TESTING-TESTING
+
+
/* check if left stick is in lower left position and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed) -> disarm
* do it only for rotary wings */
if (status.is_rotary_wing && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF &&
@@ -2246,6 +2258,13 @@ int commander_thread_main(int argc, char *argv[])
stick_off_counter = 0;
}
+ // TODO-JYW: TESTING-TESTING
+ warnx("lower right position: ON_OFF_LIMIT: %f, stick_off_counter: %d, stick_on_counter: %d",
+ (double)STICK_ON_OFF_LIMIT, stick_off_counter, stick_on_counter);
+ warnx("lower right position: sp_man.r: %.6f, sp_man.z: %.6f, status.rc_input_mode: %d",
+ (double)sp_man.r, (double)sp_man.z, status.rc_input_mode);
+ // TODO-JYW: TESTING-TESTING
+
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF ) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
@@ -2257,23 +2276,32 @@ int commander_thread_main(int argc, char *argv[])
if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL) &&
(status.main_state != vehicle_status_s::MAIN_STATE_STAB)) {
+ // TOOD-JYW: TESTING-TESTING:
+ warnx("NOT ARMING: Switch to MANUAL mode first.");
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else if (!status.condition_home_position_valid &&
geofence_action == geofence_result_s::GF_ACTION_RTL) {
+ // TOOD-JYW: TESTING-TESTING:
+ warnx("NOT ARMING: Geofence RTL requires valid home");
print_reject_arm("NOT ARMING: Geofence RTL requires valid home");
} else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
+ // TODO-JYW: TESTING-TESTING
+ warnx("attempting arming_state_transition");
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
} else {
+ // TOOD-JYW: TESTING-TESTING:
+ warnx("NOT ARMING: Configuration error");
print_reject_arm("NOT ARMING: Configuration error");
}
}
-
+ // TOOD-JYW: TESTING-TESTING:
+ warnx("on counter set to zero");
stick_on_counter = 0;
} else {
@@ -2281,9 +2309,14 @@ int commander_thread_main(int argc, char *argv[])
}
} else {
+ // TOOD-JYW: TESTING-TESTING:
+ warnx("on counter set to zero");
stick_on_counter = 0;
}
+ // TOOD-JYW: TESTING-TESTING:
+ warnx("arming_ret: %d, arming_state_changed: %d, status.main_state: %d, status.arming_state: %d", arming_ret, arming_state_changed, status.main_state, status.arming_state);
+
if (arming_ret == TRANSITION_CHANGED) {
if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
mavlink_log_info(mavlink_fd, "ARMED by RC");
@@ -2579,6 +2612,8 @@ int commander_thread_main(int argc, char *argv[])
*/
armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5 * 1000 * 1000);
}
+ // TODO-JYW: TESTING-TESTING:
+ warnx("publishing arming status, armed.armed: %d", armed.armed);
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
@@ -2602,7 +2637,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
set_tune(TONE_STOP_TUNE);
}
-
+
/* reset arm_tune_played when disarmed */
if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
@@ -2613,16 +2648,16 @@ int commander_thread_main(int argc, char *argv[])
arm_tune_played = false;
}
-
+
/* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */
hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
-
+
if (!sensor_fail_tune_played && (!status.condition_system_sensors_initialized && hotplug_timeout)) {
set_tune_override(TONE_GPS_WARNING_TUNE);
sensor_fail_tune_played = true;
status_changed = true;
}
-
+
/* update timeout flag */
if(!(hotplug_timeout == status.condition_system_hotplug_timeout)) {
status.condition_system_hotplug_timeout = hotplug_timeout;
@@ -2714,7 +2749,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
if (changed) {
bool set_normal_color = false;
bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
-
+
/* set mode */
if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
rgbled_set_mode(RGBLED_MODE_ON);
@@ -2727,7 +2762,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
rgbled_set_mode(RGBLED_MODE_BREATHE);
set_normal_color = true;
-
+
} else if (!status.condition_system_sensors_initialized && !hotplug_timeout) {
rgbled_set_mode(RGBLED_MODE_BREATHE);
set_normal_color = true;
@@ -2876,9 +2911,9 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB);
}
- }
+ }
else if(sp_man->rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){
- /* Similar to acro transitions for multirotors. FW aircraft don't need a
+ /* Similar to acro transitions for multirotors. FW aircraft don't need a
* rattitude mode.*/
if (status.is_rotary_wing) {
res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_RATTITUDE);
@@ -3165,6 +3200,12 @@ set_control_mode()
default:
break;
}
+
+ // TODO-JYW: TESTING-TESTING:
+ warnx("status.nav_state: %d", status.nav_state);
+ warnx("status.is_rotary_wing: %d", status.is_rotary_wing);
+ warnx("control_mode.flag_control_rates_enabled: %d", control_mode.flag_control_rates_enabled);
+ warnx("control_mode.flag_control_attitude_enabled: %d", control_mode.flag_control_attitude_enabled);
}
bool
@@ -3398,7 +3439,7 @@ void *commander_low_prio_loop(void *arg)
/* do esc calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
calib_ret = do_esc_calibration(mavlink_fd, &armed);
-
+
} else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */
if (status.rc_input_blocked) {
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index cad10637e5..67884716d2 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -839,6 +839,9 @@ MulticopterAttitudeControl::task_main()
continue;
}
+ // TODO-JYW: TESTING-TESTING:
+ warnx("_ctrl_state_sub is producing data");
+
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("mc att ctrl: poll error %d, %d", pret, errno);
@@ -884,6 +887,9 @@ MulticopterAttitudeControl::task_main()
}
}
+ // TODO-JYW: TESTING-TESTING:
+ warnx("_v_control_mode.flag_control_attitude_enabled: %d", _v_control_mode.flag_control_attitude_enabled);
+
if (_v_control_mode.flag_control_attitude_enabled) {
if (_ts_opt_recovery == nullptr) {
@@ -953,6 +959,9 @@ MulticopterAttitudeControl::task_main()
}
}
+ // TODO-JYW: TESTING-TESTING:
+ warnx("_v_control_mode.flag_control_rates_enabled: %d", _v_control_mode.flag_control_rates_enabled);
+
if (_v_control_mode.flag_control_rates_enabled) {
control_attitude_rates(dt);
@@ -969,8 +978,15 @@ MulticopterAttitudeControl::task_main()
_controller_status.yaw_rate_integ = _rates_int(2);
_controller_status.timestamp = hrt_absolute_time();
+ // TODO-JYW: TESTING-TESTING:
+ warnx("_actuators_0_circuit_breaker_enabled: %d", _actuators_0_circuit_breaker_enabled);
+
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub != nullptr) {
+
+ // TODO-JYW: TESTING-TESTING:
+ warnx("publishing actuator controls");
+
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
perf_end(_controller_latency_perf);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index a13b0df9d4..a44f8d76f5 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -2231,7 +2231,18 @@ Sensors::task_main()
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
- warnx("poll error %d, %d", pret, errno);
+ warnx("poll error: pret: %d, errno: %d, gyro_sub: %d", pret, errno, fds[0].fd);
+
+ // TODO-JYW: TESTING-TESTING
+ /* if the polling operation failed because no gyro sensor is available yet,
+ * then attempt to subscribe once again
+ */
+ if (_gyro_count == 0) {
+ _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
+ &raw.gyro_priority[0], &raw.gyro_errcount[0]);
+ fds[0].fd = _gyro_sub[0];
+ warnx("poll error: gyro count: %d, gyro sub: %d", _gyro_count, fds[0].fd);
+ }
continue;
}
diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp
index 6b424d8239..807519beca 100644
--- a/src/platforms/qurt/px4_layer/main.cpp
+++ b/src/platforms/qurt/px4_layer/main.cpp
@@ -220,6 +220,12 @@ int dspal_entry(int argc, char *argv[])
process_commands(apps, get_commands());
sleep(1); // give time for all commands to execute before starting external function
+ // TODO-JYW: TESTING-TESTING:
+ int default_mav_type = vehicle_status_s::VEHICLE_TYPE_QUADROTOR;
+ param_set(param_find("MAV_TYPE"), &default_mav_type);
+ PX4_INFO("setting default mav_type: %d", default_mav_type);
+ // TODO-JYW: TESTING-TESTING:
+
if (qurt_external_hook) {
qurt_external_hook();
}
--
GitLab