void Plane::one_second_loop()
{
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
// make it possible to change control channel ordering at runtime
set_control_channels();
// make it possible to change orientation at runtime
ahrs.set_orientation();
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
update_aux();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
#if AP_TERRAIN_AVAILABLE
if (should_log(MASK_LOG_GPS)) {
terrain.log_terrain_data(DataFlash);
}
#endif
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
}
/*
initialise RC output channels
*/
void Plane::init_rc_out()
{
channel_roll->enable_out();
channel_pitch->enable_out();
/*
change throttle trim to minimum throttle. This prevents a
configuration error where the user sets CH3_TRIM incorrectly and
the motor may start on power up
*/
channel_throttle->radio_trim = throttle_min();
if (arming.arming_required() != AP_Arming::YES_ZERO_PWM) {
channel_throttle->enable_out();
}
channel_rudder->enable_out();
update_aux();
RC_Channel_aux::enable_aux_servos();
// Initialization of servo outputs
RC_Channel::output_trim_all();
// setup PWM values to send if the FMU firmware dies
RC_Channel::setup_failsafe_trim_all();
// setup PX4 to output the min throttle when safety off if arming
// is setup for min on disarm
if (arming.arming_required() == AP_Arming::YES_MIN_PWM) {
hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), throttle_min());
}
}
void Rover::init_rc_in()
{
// set rc dead zones
channel_steer->set_default_dead_zone(30);
channel_throttle->set_default_dead_zone(30);
//set auxiliary ranges
update_aux();
}
void Plane::one_second_loop()
{
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
// make it possible to change control channel ordering at runtime
set_control_channels();
#if HAVE_PX4_MIXER
if (!hal.util->get_soft_armed() && (last_mixer_crc == -1)) {
// if disarmed try to configure the mixer
setup_failsafe_mixing();
}
#endif // CONFIG_HAL_BOARD
// make it possible to change orientation at runtime
ahrs.set_orientation();
adsb.set_stall_speed_cm(aparm.airspeed_min);
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
update_aux();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
#if AP_TERRAIN_AVAILABLE
if (should_log(MASK_LOG_GPS)) {
terrain.log_terrain_data(DataFlash);
}
#endif
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
// update home position if soft armed and gps position has
// changed. Update every 5s at most
if (!hal.util->get_soft_armed() &&
gps.last_message_time_ms() - last_home_update_ms > 5000 &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
last_home_update_ms = gps.last_message_time_ms();
update_home();
// reset the landing altitude correction
auto_state.land_alt_offset = 0;
}
// update error mask of sensors and subsystems. The mask uses the
// MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
// indicates that the sensor or subsystem is present but not
// functioning correctly
update_sensor_status_flags();
}
/*
once a second events
*/
void Rover::one_second_loop(void)
{
if (should_log(MASK_LOG_CURRENT)) {
Log_Write_Current();
}
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
// allow orientation change at runtime to aid config
ahrs.set_orientation();
set_control_channels();
// cope with changes to aux functions
update_aux();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
// cope with changes to mavlink system ID
mavlink_system.sysid = g.sysid_this_mav;
static uint8_t counter;
counter++;
// write perf data every 20s
if (counter % 10 == 0) {
if (scheduler.debug() != 0) {
hal.console->printf("G_Dt_max=%u\n", G_Dt_max);
}
if (should_log(MASK_LOG_PM)) {
Log_Write_Performance();
}
G_Dt_max = 0;
resetPerfData();
}
// save compass offsets once a minute
if (counter >= 60) {
if (g.compass_enabled) {
compass.save_offsets();
}
counter = 0;
}
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
// update error mask of sensors and subsystems. The mask uses the
// MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
// indicates that the sensor or subsystem is present but not
// functioning correctly
update_sensor_status_flags();
}
/*
initialise RC input channels
*/
void Plane::init_rc_in()
{
// set rc dead zones
channel_roll->set_default_dead_zone(30);
channel_pitch->set_default_dead_zone(30);
channel_rudder->set_default_dead_zone(30);
channel_throttle->set_default_dead_zone(30);
update_aux();
}
/*
initialise RC output channels for aux channels
*/
void Plane::init_rc_out_aux()
{
update_aux();
RC_Channel_aux::enable_aux_servos();
// Initialization of servo outputs
RC_Channel::output_trim_all();
// setup PWM values to send if the FMU firmware dies
RC_Channel::setup_failsafe_trim_all();
}
/*
once a second events
*/
void Rover::one_second_loop(void)
{
if (should_log(MASK_LOG_CURRENT))
Log_Write_Current();
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
// allow orientation change at runtime to aid config
ahrs.set_orientation();
set_control_channels();
// cope with changes to aux functions
update_aux();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
// cope with changes to mavlink system ID
mavlink_system.sysid = g.sysid_this_mav;
static uint8_t counter;
counter++;
// write perf data every 20s
if (counter % 10 == 0) {
if (scheduler.debug() != 0) {
hal.console->printf("G_Dt_max=%lu\n", (unsigned long)G_Dt_max);
}
if (should_log(MASK_LOG_PM))
Log_Write_Performance();
G_Dt_max = 0;
resetPerfData();
}
// save compass offsets once a minute
if (counter >= 60) {
if (g.compass_enabled) {
compass.save_offsets();
}
counter = 0;
}
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
}
/*
once a second events
*/
void Rover::one_second_loop(void)
{
// send a heartbeat
gcs().send_message(MSG_HEARTBEAT);
// allow orientation change at runtime to aid config
ahrs.set_orientation();
set_control_channels();
// cope with changes to aux functions
update_aux();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
// cope with changes to mavlink system ID
mavlink_system.sysid = g.sysid_this_mav;
static uint8_t counter;
counter++;
// save compass offsets once a minute
if (counter >= 60) {
if (g.compass_enabled) {
compass.save_offsets();
}
counter = 0;
}
// update home position if not soft armed and gps position has
// changed. Update every 1s at most
if (!hal.util->get_soft_armed() &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
update_home();
}
// update error mask of sensors and subsystems. The mask uses the
// MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
// indicates that the sensor or subsystem is present but not
// functioning correctly
update_sensor_status_flags();
}
void Plane::one_second_loop()
{
if (should_log(MASK_LOG_CURRENT))
Log_Write_Current();
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
// make it possible to change control channel ordering at runtime
set_control_channels();
// make it possible to change orientation at runtime
ahrs.set_orientation();
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
update_aux();
// determine if we are flying or not
determine_is_flying();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
#if AP_TERRAIN_AVAILABLE
if (should_log(MASK_LOG_GPS)) {
terrain.log_terrain_data(DataFlash);
}
#endif
// piggyback the status log entry on the MODE log entry flag
if (should_log(MASK_LOG_MODE)) {
Log_Write_Status();
}
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
}
void Plane::one_second_loop()
{
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
// make it possible to change control channel ordering at runtime
set_control_channels();
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
if (!hal.util->get_soft_armed() && (last_mixer_crc == -1)) {
// if disarmed try to configure the mixer
setup_failsafe_mixing();
}
#endif // CONFIG_HAL_BOARD
// make it possible to change orientation at runtime
ahrs.set_orientation();
adsb.set_stall_speed_cm(airspeed.get_airspeed_min());
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
update_aux();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
#if AP_TERRAIN_AVAILABLE
if (should_log(MASK_LOG_GPS)) {
terrain.log_terrain_data(DataFlash);
}
#endif
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
}
int
dsa_control (Attr_Sequence as, struct DSError *error, DN dn)
{
char * str;
DN dn2;
Entry theentry;
extern Entry database_root;
SFD attempt_restart();
if ( ! manager(dn) ) {
error->dse_type = DSE_SECURITYERROR;
error->ERR_SECURITY.DSE_sc_problem = DSE_SC_ACCESSRIGHTS;
return (DS_ERROR_REMOTE);
}
str = (char *) as->attr_value->avseq_av.av_struct;
#ifndef NO_STATS
LLOG (log_stat,LLOG_NOTICE,("DSA control: %s",str));
#endif
switch (*str) {
case 'd': /* -dump <directory> */
str = SkipSpace (++str);
/*
directory_dump (str, database_root);
*/
return (DS_OK);
case 't': /* -tailor <string> */
str = SkipSpace (++str);
if (dsa_tai_string (str) == OK) {
isodexport (NULLCP);
return (DS_OK);
}
break;
case 'a': /* -abort */
LLOG (log_dsap,LLOG_FATAL,("*** abort signal ***"));
dsa_abort(-1);
exit(0);
case 'b': /* -restart */
LLOG (log_dsap,LLOG_FATAL,("*** restart signal ***"));
attempt_restart (NOTOK);
exit(0); /* should not be reached */
case 'r': /* -refresh <entry> */
str = SkipSpace (++str);
if (lexequ (str,"root") == 0)
dn2= NULLDN;
else if ((dn2 = str2dn (str)) == NULLDN)
break;
if (refresh_from_disk (dn2) == OK)
return (DS_OK);
break;
case 'f': /* -resync <entry> */
str = SkipSpace (++str);
if (lexequ (str,"root") == 0)
dn2= NULLDN;
else if ((dn2 = str2dn (str)) == NULLDN)
break;
if ((theentry = local_find_entry (dn2,FALSE)) != NULLENTRY)
#ifdef TURBO_DISK
{
Entry akid = (Entry) avl_getone(theentry->e_children);
if (turbo_writeall (akid) == OK)
return (DS_OK);
}
#else
{
Entry akid = (Entry) avl_getone(theentry->e_children);
if (journal (akid) == OK)
return (DS_OK);
}
#endif
break;
case 'l': /* -lock <entry> */
str = SkipSpace (++str);
if (lexequ (str,"root") == 0)
dn2 = NULLDN;
else if ((dn2 = str2dn (str)) == NULLDN)
break;
if ((theentry = local_find_entry (dn2,FALSE)) != NULLENTRY) {
theentry->e_lock = TRUE;
return (DS_OK);
}
break;
case 'u': /* -unlock <entry> */
str = SkipSpace (++str);
if (lexequ (str,"root") == 0)
dn2 = NULLDN;
else if ((dn2 = str2dn (str)) == NULLDN)
break;
if ((theentry = local_find_entry (dn2,FALSE)) != NULLENTRY) {
theentry->e_lock = FALSE;
return (DS_OK);
}
break;
case 's': /* -slave */
/*
* When we go async return of OK will mean that a getedb
* operation has been scheduled, NOT that it has succeeded.
*/
str = SkipSpace (++str);
if (*str == NULL) {
slave_update();
return DS_OK;
}
if (lexequ (str, "shadow") == 0) {
shadow_update();
return DS_OK;
}
if (lexequ (str, "root") == 0)
dn2 = NULLDN;
else if ((dn2 = str2dn (str)) == NULLDN)
break;
if (update_aux (dn2, dn2 == NULLDN) == OK)
return DS_OK;
break;
default:
break;
}
error->dse_type = DSE_SERVICEERROR;
error->ERR_SERVICE.DSE_sv_problem = DSE_SV_UNWILLINGTOPERFORM;
return (DS_ERROR_REMOTE);
}
int
new_dsa_control (Attr_Sequence as, struct DSError *error, DN dn)
{
struct dsa_control * item ;
char * tmp_ptr ;
DN dn2;
Entry theentry;
extern Entry database_root;
SFD attempt_restart();
/* Return some silly error to distinguish it from the other dsa_control */
if ( ! manager(dn) ) {
error->dse_type = DSE_SECURITYERROR ;
error->ERR_SECURITY.DSE_sc_problem = DSE_SC_PROTECTIONREQUIRED ;
return (DS_ERROR_REMOTE) ;
}
item = (struct dsa_control *) as->attr_value->avseq_av.av_struct ;
switch (item->dsa_control_option) {
case (CONTROL_CHANGETAILOR) : { /* -tailor <string> */
tmp_ptr = qb2str(item->un.changeTailor) ;
if (dsa_tai_string (tmp_ptr) == OK) {
isodexport (NULLCP);
return (DS_OK);
}
break;
}
case(CONTROL_STOPDSA): { /* -abort */
LLOG(log_dsap,LLOG_FATAL,("*** abort signal ***")) ;
stop_listeners() ;
exit(0) ;
}
case (CONTROL_REFRESH): { /* -refresh <entry> */
if (item->un.refresh->offset == DN_PRESENT)
if (refresh_from_disk (item->un.refresh->un.selectedDN) == OK)
return (DS_OK);
break;
}
case(CONTROL_RESYNCH): { /* -resync <entry> */
if (item->un.resynch->offset == DN_PRESENT)
dn2 = item->un.resynch->un.selectedDN ;
else
break ;
if ((theentry = local_find_entry (dn2, FALSE)) != NULLENTRY)
#ifdef TURBO_DISK
{
Entry akid = (Entry) avl_getone(theentry->e_children);
if (turbo_writeall (akid) == OK)
return (DS_OK);
}
#else
{
Entry akid = (Entry) avl_getone(theentry->e_children);
if (journal (akid) == OK)
return (DS_OK);
}
#endif
break;
}
case (CONTROL_LOCKDSA): { /* -lock <entry> */
if (item->un.resynch->offset == DN_PRESENT)
dn2 = item->un.resynch->un.selectedDN ;
else
break ;
if ((theentry = local_find_entry (dn2,FALSE)) != NULLENTRY) {
theentry->e_lock = TRUE;
return (DS_OK);
}
break;
}
case (CONTROL_UNLOCK): { /* -unlock <entry> */
if (item->un.resynch->offset == DN_PRESENT)
dn2 = item->un.resynch->un.selectedDN ;
else
break ;
if ((theentry = local_find_entry (dn2,FALSE)) != NULLENTRY) {
theentry->e_lock = FALSE;
return (DS_OK);
}
break;
}
case (CONTROL_SETLOGLEVEL): { /* Set the Logging Level */
tmp_ptr = qb2str(item->un.setLogLevel) ;
/* What kind of stuff is needed here?!! */
return (DS_OK);
break;
}
case (CONTROL_UPDATESLAVEEDBS): { /* -slave */
/*
* When we go async return of OK will mean that a getedb
* operation has been scheduled, NOT that it has succeeded.
*/
tmp_ptr = qb2str(item->un.updateSlaveEDBs) ;
if (lexequ (tmp_ptr, "all") == 0) {
slave_update();
return DS_OK;
}
if (lexequ (tmp_ptr, "shadow") == 0) {
shadow_update();
return DS_OK;
}
if (lexequ (tmp_ptr, "root") == 0) {
dn2 = NULLDN;
} else {
if ((dn2 = str2dn (tmp_ptr)) == NULLDN)
break;
}
if (update_aux (dn2, dn2 == NULLDN) == OK)
return DS_OK;
break;
}
default:
break;
}
error->dse_type = DSE_SERVICEERROR;
error->ERR_SERVICE.DSE_sv_problem = DSE_SV_UNWILLINGTOPERFORM;
return (DS_ERROR_REMOTE);
}
void Rover::init_rc_out()
{
// set auxiliary ranges
update_aux();
}
