Plane: Don't initilize motors with a trim value

This commit is contained in:
Michael du Breuil 2018-08-22 00:17:41 -07:00 committed by Andrew Tridgell
parent d9b72f6821
commit e4bbcd5ee3
4 changed files with 3 additions and 20 deletions

View File

@ -238,15 +238,6 @@ void Plane::afs_fs_check(void)
afs.check(failsafe.last_heartbeat_ms, geofence_breached(), failsafe.AFS_last_valid_rc_ms); afs.check(failsafe.last_heartbeat_ms, geofence_breached(), failsafe.AFS_last_valid_rc_ms);
} }
/*
update aux servo mappings
*/
void Plane::update_aux(void)
{
SRV_Channels::enable_aux_servos();
}
void Plane::one_second_loop() void Plane::one_second_loop()
{ {
// send a heartbeat // send a heartbeat
@ -271,7 +262,7 @@ void Plane::one_second_loop()
// sync MAVLink system ID // sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav; mavlink_system.sysid = g.sysid_this_mav;
update_aux(); SRV_Channels::enable_aux_servos();
// update notify flags // update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false); AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);

View File

@ -965,7 +965,6 @@ private:
void servos_twin_engine_mix(); void servos_twin_engine_mix();
void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle); void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle);
bool allow_reverse_thrust(void); bool allow_reverse_thrust(void);
void update_aux();
void update_is_flying_5Hz(void); void update_is_flying_5Hz(void);
void crash_detection_update(void); void crash_detection_update(void);
bool in_preLaunch_flight_stage(void); bool in_preLaunch_flight_stage(void);

View File

@ -90,18 +90,15 @@ void Plane::init_rc_out_main()
*/ */
void Plane::init_rc_out_aux() void Plane::init_rc_out_aux()
{ {
update_aux();
SRV_Channels::enable_aux_servos(); SRV_Channels::enable_aux_servos();
SRV_Channels::cork(); SRV_Channels::cork();
// Initialization of servo outputs
SRV_Channels::output_trim_all();
servos_output(); servos_output();
// setup PWM values to send if the FMU firmware dies // setup PWM values to send if the FMU firmware dies
SRV_Channels::setup_failsafe_trim_all(); // allows any VTOL motors to shut off
SRV_Channels::setup_failsafe_trim_all_non_motors();
} }
/* /*

View File

@ -227,10 +227,6 @@ void Plane::dspoiler_update(void)
*/ */
void Plane::set_servos_idle(void) void Plane::set_servos_idle(void)
{ {
if (auto_state.idle_wiggle_stage == 0) {
SRV_Channels::output_trim_all();
return;
}
int16_t servo_value = 0; int16_t servo_value = 0;
// move over full range for 2 seconds // move over full range for 2 seconds
auto_state.idle_wiggle_stage += 2; auto_state.idle_wiggle_stage += 2;