INA226 support for CubeOrangePlus

boards/cubepilot/cubeorangeplus/init/rc.board_defaults

@@ -9,6 +9,9 @@ param set-default BAT2_V_DIV 10.1
 param set-default BAT1_A_PER_V 17
 param set-default BAT2_A_PER_V 17
 
+# Enable INA226
+param set-default SENS_EN_INA226 1
+
 # Disable IMU thermal control
 param set-default SENS_EN_THERMAL 0
 

boards/cubepilot/cubeorangeplus/init/rc.board_sensors

@@ -41,3 +41,9 @@ else
 fi
 
 ms5611 -s -b 1 start
+
+if param compare SENS_EN_INA226 1
+then
+	# Start INA226 driver
+	ina226 -X -b 2 -t 1 -k start
+fi

make-scout-bundle.sh

@@ -0,0 +1,20 @@
+#!/usr/bin/env bash
+
+make clean
+make ark_fmu-v6x_default
+make px4_fmu-v6x
+
+tag=$(git describe --tags --match "*" --dirty)
+
+tmp=$(mktemp -d)
+mkdir -p $tmp/"px4-custom-$tag"
+cp build/ark_fmu-v6x_default/*.px4 $tmp/"px4-custom-$tag"
+cp build/px4_fmu-v6x_default/*.px4 $tmp/"px4-custom-$tag"
+
+echo "rev: $(git rev-parse HEAD)" > $tmp/"px4-custom-$tag"/src-info
+echo "remote: $(git remote get-url origin)" >> $tmp/"px4-custom-$tag"/src-info
+
+dir=$(pwd)
+pushd $tmp
+tar -czvf "$dir/px4-custom-$tag.tar.gz" "px4-custom-$tag"
+popd

msg/ManualControlSwitches.msg

@@ -25,6 +25,7 @@ uint8 offboard_switch            # offboard 2 position switch (optional): _NORMA
 uint8 kill_switch                # throttle kill: _NORMAL_, KILL
 uint8 gear_switch                # landing gear switch: _DOWN_, UP
 uint8 transition_switch          # VTOL transition switch: _HOVER, FORWARD_FLIGHT
+uint8 manual_estop_switch	 # Rising edge triggers manual control override behavior
 
 uint8 photo_switch               # Photo trigger switch
 uint8 video_switch               # Photo trigger switch

msg/RcChannels.msg

@@ -29,13 +29,14 @@ uint8 FUNCTION_FLTBTN_SLOT_5 = 25
 uint8 FUNCTION_FLTBTN_SLOT_6 = 26
 uint8 FUNCTION_ENGAGE_MAIN_MOTOR = 27
 uint8 FUNCTION_PAYLOAD_POWER = 28
+uint8 FUNCTION_RC_ESTOP = 29
 
 uint8 FUNCTION_FLTBTN_SLOT_COUNT = 6
 
 uint64 timestamp_last_valid					# Timestamp of last valid RC signal
 float32[18] channels						# Scaled to -1..1 (throttle: 0..1)
 uint8 channel_count						# Number of valid channels
-int8[29] function						# Functions mapping
+int8[30] function						# Functions mapping
 uint8 rssi							# Receive signal strength index
 bool signal_lost						# Control signal lost, should be checked together with topic timeout
 uint32 frame_drop_count						# Number of dropped frames

src/modules/manual_control/ManualControl.cpp

@@ -107,6 +107,26 @@ void ManualControl::processInput(hrt_abstime now)
 		}
 	}
 
+	const bool rc_estop_engaged = _selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC &&
+				      _rc_estop_engaged;
+
+	if (_rc_estop_published_last != rc_estop_engaged || hrt_elapsed_time(&_rc_estop_published_last_time) > 1000_ms) {
+		debug_key_value_s kv;
+		kv.timestamp = hrt_absolute_time();
+
+
+		memset(kv.key, 0, sizeof(kv.key));
+		size_t key_len = strlen(ESTOP_DEBUG_VALUE_KEY);
+		size_t max_len = sizeof(kv.key) - 1;
+		size_t len_to_copy = key_len > max_len ? max_len : key_len;
+		memcpy(kv.key, ESTOP_DEBUG_VALUE_KEY, len_to_copy);
+		kv.value = rc_estop_engaged ? 1.f : 0.f;
+		_debug_kv_pub.publish(kv);
+		_rc_estop_published_last_time = kv.timestamp;
+		_rc_estop_published_last = rc_estop_engaged;
+	}
+
+
 	if (_selector.setpoint().valid) {
 		_published_invalid_once = false;
 
@@ -116,13 +136,88 @@ void ManualControl::processInput(hrt_abstime now)
 		const float dt_s = (now - _timestamp_last_loop) / 1e6f;
 		const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get();
 
+
+
 		_selector.setpoint().sticks_moving = (fabsf(_roll_diff.update(_selector.setpoint().roll, dt_s)) > minimum_stick_change)
 						     || (fabsf(_pitch_diff.update(_selector.setpoint().pitch, dt_s)) > minimum_stick_change)
 						     || (fabsf(_yaw_diff.update(_selector.setpoint().yaw, dt_s)) > minimum_stick_change)
 						     || (fabsf(_throttle_diff.update(_selector.setpoint().throttle, dt_s)) > minimum_stick_change);
 
+		if (_selector.setpoint().sticks_moving) {
+			_time_last_sticks_actually_moved = hrt_absolute_time();
+		}
+
+		auto setpoint = _selector.setpoint();
+
+		if (_selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC) {
+			const bool fake_stick_movement = _rc_estop_engaged &&
+							 (hrt_absolute_time() <= (_time_rc_estop_engaged + 2500_ms));
+
+			if (fake_stick_movement) {
+				setpoint.sticks_moving = true;
+			}
+
+			auto neutralize_inputs = [&]() {
+				if (hrt_elapsed_time(&_time_last_neutral_sticks_msg) > 10000_ms) {
+					_time_last_neutral_sticks_msg = hrt_absolute_time();
+					orb_advert_t mavlink_log_pub = nullptr;
+					mavlink_log_info(&mavlink_log_pub, "R/C E-Stop: Overriding sticks with neutral until real movement");
+					events::send(events::ID("manual_control_rc_e_stop_movment_not_detected"), {events::Log::Info, events::LogInternal::Info},
+						     "ManualControl: R/C E-Stop: Overriding sticks with neutral until real movement");
+				}
+
+				setpoint.roll = 0;
+				setpoint.pitch = 0;
+				setpoint.yaw = 0;
+				setpoint.throttle = 0;
+			};
+
+			switch (_rc_estop_neutral_stick_state) {
+			case RCEStopNeutralStickState::IDLE:
+				if (_rc_estop_engaged) {
+					neutralize_inputs();
+					_rc_estop_neutral_stick_state = RCEStopNeutralStickState::HOLD_NEUTRAL;
+					_time_rc_estop_neutral_stick_override_engaged = _time_rc_estop_engaged;
+				}
+
+				break;
+
+			case RCEStopNeutralStickState::HOLD_NEUTRAL:
+				if (_time_last_sticks_actually_moved < _time_rc_estop_neutral_stick_override_engaged) {
+					//Sticks haven't moved since ESTOP switch engaged
+					neutralize_inputs();
+
+				} else {
+					_rc_estop_neutral_stick_state = RCEStopNeutralStickState::MOVEMENT_DETECTED;
+					orb_advert_t mavlink_log_pub = nullptr;
+					mavlink_log_info(&mavlink_log_pub, "R/C E-Stop stick movement detected. No longer overriding with neutral sticks");
+					events::send(events::ID("manual_control_rc_e_stop_movment"), {events::Log::Info, events::LogInternal::Info},
+						     "ManualControl: R/C E-Stop stick movement detected. No longer overriding with neutral sticks");
+				}
+
+				break;
+
+			case RCEStopNeutralStickState::MOVEMENT_DETECTED:
+				if (_rc_estop_engaged && (_time_rc_estop_engaged != _time_rc_estop_neutral_stick_override_engaged)) {
+					//Somehow, the switch was re-engaged. Go back to HOLD_NEUTRAL
+					neutralize_inputs();
+					_rc_estop_neutral_stick_state = RCEStopNeutralStickState::HOLD_NEUTRAL;
+
+				} else if (!_rc_estop_engaged) {
+					_rc_estop_neutral_stick_state = RCEStopNeutralStickState::IDLE;
+				}
+
+				break;
+			}
+
+		} else {
+			_rc_estop_neutral_stick_state = RCEStopNeutralStickState::IDLE;
+		}
+
+
 		_selector.setpoint().timestamp = now;
-		_manual_control_setpoint_pub.publish(_selector.setpoint());
+		setpoint.timestamp = now;
+		_manual_control_setpoint_pub.publish(setpoint);
 
 		// Attach scheduling to new samples of the chosen input
 		const int instance = _selector.instance();
@@ -228,6 +323,33 @@ void ManualControl::processSwitches(hrt_abstime &now)
 					}
 				}
 
+
+				if (switches.manual_estop_switch != _previous_switches.manual_estop_switch) {
+					switch (switches.manual_estop_switch) {
+					case manual_control_switches_s::SWITCH_POS_ON: {
+							orb_advert_t mavlink_log_pub = nullptr;
+							mavlink_log_info(&mavlink_log_pub, "R/C E-Stop engaged. Simulating stick movement for 2.5 seconds");
+							events::send(events::ID("manual_control_rc_e_stop"), {events::Log::Info, events::LogInternal::Info},
+								     "ManualControl: R/C E-stop engaged. Simulating stick movement for 2.5 seconds");
+							_rc_estop_engaged = true;
+							_time_rc_estop_engaged = hrt_absolute_time();
+						}
+						break;
+
+					case manual_control_switches_s::SWITCH_POS_OFF: {
+							_rc_estop_engaged = false;
+							_time_rc_estop_engaged = 0;
+							break;
+						}
+
+					case manual_control_switches_s::SWITCH_POS_NONE:
+					default:
+						_rc_estop_engaged = false;
+						_time_rc_estop_engaged = 0;
+						break;
+					}
+				}
+
 				if (switches.kill_switch != _previous_switches.kill_switch) {
 					if (switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) {
 						sendActionRequest(action_request_s::ACTION_KILL, action_request_s::SOURCE_RC_SWITCH);
@@ -289,12 +411,15 @@ void ManualControl::processSwitches(hrt_abstime &now)
 				evaluateModeSlot(switches.mode_slot);
 			}
 
+
 			_previous_switches = switches;
 			_previous_switches_initialized = true;
 		}
 
 	} else {
 		// Don't react on switch changes while RC was not in use
+		_rc_estop_engaged = false;
+		_time_rc_estop_engaged = 0;
 		_previous_switches_initialized = false;
 	}
 }

src/modules/manual_control/ManualControl.hpp

@@ -42,6 +42,7 @@
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/topics/action_request.h>
+#include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/landing_gear.h>
 #include <uORB/topics/manual_control_switches.h>
 #include <uORB/topics/manual_control_setpoint.h>
@@ -105,6 +106,7 @@ class ManualControl : public ModuleBase<ManualControl>, public ModuleParams, pub
 		{this, ORB_ID(manual_control_input), 2},
 	};
 	uORB::SubscriptionCallbackWorkItem _manual_control_switches_sub{this, ORB_ID(manual_control_switches)};
+	uORB::Subscription _manual_control_switches_sub_offboard_consent{ORB_ID(manual_control_switches)};
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
@@ -115,6 +117,18 @@ class ManualControl : public ModuleBase<ManualControl>, public ModuleParams, pub
 	ManualControlSelector _selector;
 
 	hrt_abstime _timestamp_last_loop{0};
+
+	enum class RCEStopNeutralStickState {
+		IDLE,
+		HOLD_NEUTRAL,
+		MOVEMENT_DETECTED
+	};
+	bool _rc_estop_engaged{false};
+	RCEStopNeutralStickState _rc_estop_neutral_stick_state{RCEStopNeutralStickState::IDLE};
+	hrt_abstime _time_rc_estop_engaged{0};
+	hrt_abstime _time_rc_estop_neutral_stick_override_engaged{0};
+	hrt_abstime _time_last_sticks_actually_moved{0};
+	hrt_abstime _time_last_neutral_sticks_msg{0};
 	int _previous_manual_control_input_instance{-1};
 	bool _previous_switches_initialized{false};
 	manual_control_switches_s _previous_switches{};
@@ -142,6 +156,15 @@ class ManualControl : public ModuleBase<ManualControl>, public ModuleParams, pub
 	bool _rotary_wing{false};
 	bool _vtol{false};
 
+
+	static constexpr auto ESTOP_DEBUG_VALUE_KEY = "RCESTOPEN";
+
+	uORB::Publication<debug_key_value_s> _debug_kv_pub{ORB_ID(debug_key_value)};
+	bool _rc_estop_published_last{false};
+	hrt_abstime _rc_estop_published_last_time{0};
+
+
+
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::COM_RC_IN_MODE>) _param_com_rc_in_mode,
 		(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,

src/modules/mavlink/streams/BATTERY_STATUS.hpp

@@ -144,28 +144,32 @@ class MavlinkStreamBatteryStatus : public MavlinkStream
 					// We don't know the cell count or we don't know the indpendent cell voltages,
 					// so we report the total voltage in the first cell, or cell(s) if the voltage
 					// doesn't "fit" in the UINT16.
-					if (battery_status.cell_count == 0 || battery_status.voltage_cell_v[0] < 0.0001f) {
+					const bool voltage_valid = PX4_ISFINITE(battery_status.voltage_v) && (battery_status.voltage_v > 0.f);
+
+					if ((battery_status.cell_count == 0 || battery_status.voltage_cell_v[0] < 0.0001f) && voltage_valid) {
 						// If it doesn't fit, we have to split it into UINT16-1 chunks and the remaining
 						// voltage for the subsequent field.
 						// This won't work for voltages of more than 655 volts.
-						const int num_fields_required = static_cast<int>(battery_status.voltage_v * 1000.f) / (UINT16_MAX - 1) + 1;
+						const int total_mv = (int)lroundf(battery_status.voltage_v * 1000.f);
+						const int num_fields_required = total_mv / (UINT16_MAX - 1) + 1;
 
 						if (num_fields_required <= mavlink_cell_slots) {
-							float remaining_voltage = battery_status.voltage_v * 1000.f;
+							float remaining_voltage = (float)total_mv;
 
 							for (int i = 0; i < num_fields_required - 1; ++i) {
 								bat_msg.voltages[i] = UINT16_MAX - 1;
 								remaining_voltage -= UINT16_MAX - 1;
 							}
 
-							bat_msg.voltages[num_fields_required - 1] = remaining_voltage;
+							bat_msg.voltages[num_fields_required - 1] =
+								(uint16_t)math::min((float)(UINT16_MAX - 1), math::max(0.f, remaining_voltage));
 
 						} else {
 							// Leave it default/unknown. We're out of spec.
 						}
 
 
-					} else {
+					} else if (battery_status.voltage_cell_v[0] >= 0.0001f) {
 						static constexpr int uorb_cell_slots =
 							(sizeof(battery_status.voltage_cell_v) / sizeof(battery_status.voltage_cell_v[0]));