Copter: Warn user with motors while copter is arming

This commit is contained in:
Jonathan Challinger 2013-06-22 19:02:26 -07:00 committed by Randy Mackay
parent ae2b1e3b7e
commit 2725f219cb
4 changed files with 20 additions and 3 deletions

View File

@ -110,6 +110,9 @@ static void init_arm_motors()
// disable cpu failsafe because initialising everything takes a while
failsafe_disable();
motors.enable();
motors.output_unsafe();
#if LOGGING_ENABLED == ENABLED
// start dataflash
@ -178,9 +181,6 @@ static void init_arm_motors()
piezo_beep_twice();
#endif
// enable output to motors
output_min();
// finally actually arm the motors
motors.armed(true);

View File

@ -453,6 +453,19 @@ void AP_MotorsMatrix::output_disarmed()
output_min();
}
void AP_MotorsMatrix::output_unsafe()
{
int8_t i;
// fill the motor_out[] array for HIL use and send minimum value to each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
motor_out[i] = _rc_throttle->radio_min + constrain_int16(_throttle_unsafe,0,AP_MOTORS_THROTTLE_UNSAFE_MAX);;
hal.rcout->write(_motor_to_channel_map[i], motor_out[i]);
}
}
}
// output_disarmed - sends commands to the motors
void AP_MotorsMatrix::output_test()
{

View File

@ -50,6 +50,8 @@ public:
// add_motor using just position and yaw_factor (or prop direction)
void add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order);
void output_unsafe();
// remove_motor
void remove_motor(int8_t motor_num);

View File

@ -85,6 +85,8 @@ public:
// enable - starts allowing signals to be sent to motors
virtual void enable() = 0;
virtual void output_unsafe() = 0;
// arm, disarm or check status status of motors
bool armed() { return _armed; };