dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

added option for esc calibration

This commit is contained in:
Roman Bapst 2015-04-28 14:55:38 +02:00
parent dd0ed9b446
commit c3111ecadf
1 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/drivers/px4io/px4io.cpp
View File

@ -302,6 +302,7 @@ private:
float _battery_mamphour_total;///< amp hours consumed so far float _battery_mamphour_total;///< amp hours consumed so far
uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp
bool _cb_flighttermination; ///< true if the flight termination circuit breaker is enabled bool _cb_flighttermination; ///< true if the flight termination circuit breaker is enabled
bool _in_esc_calibration_mode; ///< do not send control outputs to IO (used for esc calibration)
int32_t _rssi_pwm_chan; ///< RSSI PWM input channel int32_t _rssi_pwm_chan; ///< RSSI PWM input channel
int32_t _rssi_pwm_max; ///< max RSSI input on PWM channel int32_t _rssi_pwm_max; ///< max RSSI input on PWM channel
@ -529,6 +530,7 @@ PX4IO::PX4IO(device::Device *interface) :
_battery_mamphour_total(0), _battery_mamphour_total(0),
_battery_last_timestamp(0), _battery_last_timestamp(0),
_cb_flighttermination(true), _cb_flighttermination(true),
_in_esc_calibration_mode(false),
_rssi_pwm_chan(0), _rssi_pwm_chan(0),
_rssi_pwm_max(0), _rssi_pwm_max(0),
_rssi_pwm_min(0) _rssi_pwm_min(0)
@ -1167,9 +1169,14 @@ PX4IO::io_set_control_state(unsigned group)
break; break;
} }
if (!changed) { if (!changed && (!_in_esc_calibration_mode || group != 0)) {
return -1; return -1;
} }
else if (_in_esc_calibration_mode && group == 0) {
// modify controls to get max pwm (full thrust) on every esc
memset(&controls, 0, sizeof(controls));
controls.control[3] = 1.0f; // set maximum thrust
}
for (unsigned i = 0; i < _max_controls; i++) { for (unsigned i = 0; i < _max_controls; i++) {
regs[i] = FLOAT_TO_REG(controls.control[i]); regs[i] = FLOAT_TO_REG(controls.control[i]);
@ -1188,12 +1195,14 @@ PX4IO::io_set_arming_state()
int have_armed = orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); int have_armed = orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
int have_control_mode = orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode); int have_control_mode = orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
_in_esc_calibration_mode = armed.in_esc_calibration_mode;
uint16_t set = 0; uint16_t set = 0;
uint16_t clear = 0; uint16_t clear = 0;
if (have_armed == OK) { if (have_armed == OK) {
if (armed.armed) { _in_esc_calibration_mode = armed.in_esc_calibration_mode;
if (armed.armed || _in_esc_calibration_mode) {
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
} else { } else {
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;