mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 05:43:33 +08:00
px4io:Removed FMUv1 hardware dependancies
Removed conditional compilation for FMUv1 and defaulted to FMUv2.
This commit is contained in:
David Sidrane
committed by
Daniel Agar
Daniel Agar
parent
5782e5c5fd
commit
e07cb71a11
@@ -228,28 +228,6 @@ public:
|
||||
_test_fmu_fail = is_fail;
|
||||
};
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
/**
|
||||
* Set the DSM VCC is controlled by relay one flag
|
||||
*
|
||||
* @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
|
||||
*/
|
||||
inline void set_dsm_vcc_ctl(bool enable)
|
||||
{
|
||||
_dsm_vcc_ctl = enable;
|
||||
};
|
||||
|
||||
/**
|
||||
* Get the DSM VCC is controlled by relay one flag
|
||||
*
|
||||
* @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
|
||||
*/
|
||||
inline bool get_dsm_vcc_ctl()
|
||||
{
|
||||
return _dsm_vcc_ctl;
|
||||
};
|
||||
#endif
|
||||
|
||||
inline uint16_t system_status() const {return _status;}
|
||||
|
||||
private:
|
||||
@@ -328,10 +306,6 @@ private:
|
||||
float _last_throttle; ///< last throttle value for battery calculation
|
||||
bool _test_fmu_fail; ///< To test what happens if IO looses FMU
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Trampoline to the worker task
|
||||
*/
|
||||
@@ -474,16 +448,6 @@ private:
|
||||
*/
|
||||
void dsm_bind_ioctl(int dsmMode);
|
||||
|
||||
/**
|
||||
* Handle a battery update from IO.
|
||||
*
|
||||
* Publish IO battery information if necessary.
|
||||
*
|
||||
* @param vbatt vbatt register
|
||||
* @param ibatt ibatt register
|
||||
*/
|
||||
void io_handle_battery(uint16_t vbatt, uint16_t ibatt);
|
||||
|
||||
/**
|
||||
* Handle a servorail update from IO.
|
||||
*
|
||||
@@ -564,10 +528,6 @@ PX4IO::PX4IO(device::Device *interface) :
|
||||
_analog_rc_rssi_volt(-1.0f),
|
||||
_last_throttle(0.0f),
|
||||
_test_fmu_fail(false)
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
, _dsm_vcc_ctl(false)
|
||||
#endif
|
||||
|
||||
{
|
||||
/* we need this potentially before it could be set in task_main */
|
||||
g_dev = this;
|
||||
@@ -1777,43 +1737,6 @@ PX4IO::io_handle_alarms(uint16_t alarms)
|
||||
return 0;
|
||||
}
|
||||
|
||||
void
|
||||
PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
|
||||
{
|
||||
/* only publish if battery has a valid minimum voltage */
|
||||
if (vbatt <= 4900) {
|
||||
return;
|
||||
}
|
||||
|
||||
battery_status_s battery_status;
|
||||
|
||||
const hrt_abstime timestamp = hrt_absolute_time();
|
||||
/* voltage is scaled to mV */
|
||||
const float voltage_v = vbatt / 1000.0f;
|
||||
|
||||
/*
|
||||
ibatt contains the raw ADC count, as 12 bit ADC
|
||||
value, with full range being 3.3v
|
||||
*/
|
||||
float current_a = ibatt * (3.3f / 4096.0f) * _battery_amp_per_volt;
|
||||
current_a += _battery_amp_bias;
|
||||
|
||||
_battery.updateBatteryStatus(timestamp, voltage_v, current_a, true, true, 0,
|
||||
_last_throttle,
|
||||
_armed, &battery_status);
|
||||
|
||||
/* the announced battery status would conflict with the simulated battery status in HIL */
|
||||
if (!(_pub_blocked)) {
|
||||
/* lazily publish the battery voltage */
|
||||
if (_to_battery != nullptr) {
|
||||
orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
|
||||
|
||||
} else {
|
||||
_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi)
|
||||
{
|
||||
@@ -1861,13 +1784,7 @@ PX4IO::io_get_status()
|
||||
io_handle_status(regs[0]);
|
||||
io_handle_alarms(regs[1]);
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
io_handle_battery(regs[2], regs[3]);
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
io_handle_vservo(regs[4], regs[5]);
|
||||
#endif
|
||||
|
||||
return ret;
|
||||
}
|
||||
@@ -2477,20 +2394,11 @@ PX4IO::print_status(bool extended_status)
|
||||
((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : ""),
|
||||
((arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) ? " OVERRIDE_IMMEDIATE" : "")
|
||||
);
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE),
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
|
||||
#endif
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
printf("rates 0x%04x default %u alt %u sbus %u\n",
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE),
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SBUS_RATE));
|
||||
#endif
|
||||
printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
|
||||
|
||||
for (unsigned group = 0; group < 4; group++) {
|
||||
@@ -2908,68 +2816,20 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
||||
}
|
||||
|
||||
case GPIO_RESET: {
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
uint32_t bits = (1 << _max_relays) - 1;
|
||||
|
||||
/* don't touch relay1 if it's controlling RX vcc */
|
||||
if (_dsm_vcc_ctl) {
|
||||
bits &= ~PX4IO_P_SETUP_RELAYS_POWER1;
|
||||
}
|
||||
|
||||
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0);
|
||||
#endif
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
ret = -EINVAL;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
||||
case GPIO_SET:
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
arg &= ((1 << _max_relays) - 1);
|
||||
|
||||
/* don't touch relay1 if it's controlling RX vcc */
|
||||
if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) {
|
||||
ret = -EINVAL;
|
||||
break;
|
||||
}
|
||||
|
||||
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
|
||||
#endif
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
ret = -EINVAL;
|
||||
#endif
|
||||
break;
|
||||
|
||||
case GPIO_CLEAR:
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
arg &= ((1 << _max_relays) - 1);
|
||||
|
||||
/* don't touch relay1 if it's controlling RX vcc */
|
||||
if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) {
|
||||
ret = -EINVAL;
|
||||
break;
|
||||
}
|
||||
|
||||
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
|
||||
#endif
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
ret = -EINVAL;
|
||||
#endif
|
||||
break;
|
||||
|
||||
case GPIO_GET:
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
*(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS);
|
||||
|
||||
if (*(uint32_t *)arg == _io_reg_get_error) {
|
||||
ret = -EIO;
|
||||
}
|
||||
|
||||
#endif
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
ret = -EINVAL;
|
||||
#endif
|
||||
break;
|
||||
|
||||
case MIXERIOCGETOUTPUTCOUNT:
|
||||
@@ -3221,8 +3081,6 @@ get_interface()
|
||||
{
|
||||
device::Device *interface = nullptr;
|
||||
|
||||
#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
|
||||
#ifdef PX4IO_SERIAL_BASE
|
||||
interface = PX4IO_serial_interface();
|
||||
#endif
|
||||
@@ -3231,8 +3089,6 @@ get_interface()
|
||||
goto got;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef PX4_I2C_OBDEV_PX4IO
|
||||
interface = PX4IO_i2c_interface();
|
||||
#endif
|
||||
@@ -3290,17 +3146,6 @@ start(int argc, char *argv[])
|
||||
errx(1, "driver init failed");
|
||||
}
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
int dsm_vcc_ctl;
|
||||
|
||||
if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) {
|
||||
if (dsm_vcc_ctl) {
|
||||
g_dev->set_dsm_vcc_ctl(true);
|
||||
g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
exit(0);
|
||||
}
|
||||
|
||||
@@ -3417,14 +3262,6 @@ bind(int argc, char *argv[])
|
||||
errx(1, "px4io must be started first");
|
||||
}
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
|
||||
if (!g_dev->get_dsm_vcc_ctl()) {
|
||||
errx(1, "DSM bind feature not enabled");
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
if (argc < 3) {
|
||||
errx(0, "needs argument, use dsm2, dsmx or dsmx8");
|
||||
}
|
||||
@@ -3451,9 +3288,6 @@ bind(int argc, char *argv[])
|
||||
errx(1, "system must not be armed");
|
||||
}
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1.");
|
||||
#endif
|
||||
g_dev->ioctl(nullptr, DSM_BIND_START, pulses);
|
||||
|
||||
exit(0);
|
||||
|
||||
Reference in New Issue
Block a user