//! Start a given plan
//! @param[in] plan_id name of the plan to execute
//! @param[in] spec plan specification message if any
//! @param[in] flags plan control flags
//! @return false if previously executing maneuver was not stopped
bool
startPlan(const std::string& plan_id, const IMC::Message* spec, uint16_t flags)
{
bool stopped = stopPlan(true);
changeMode(IMC::PlanControlState::PCS_INITIALIZING,
DTR("plan initializing: ") + plan_id);
if (!loadPlan(plan_id, spec, true))
return stopped;
changeLog(plan_id);
// Flag the plan as starting
if (initMode() || execMode())
{
if (!stopped)
m_plan->planStopped();
m_plan->planStarted();
}
dispatch(m_spec);
if ((flags & IMC::PlanControl::FLG_CALIBRATE) &&
m_args.do_calib)
{
if (!startCalibration())
return stopped;
}
else
{
IMC::PlanManeuver* pman = m_plan->loadStartManeuver();
startManeuver(pman);
if (execMode())
{
onSuccess(m_last_event);
}
else
{
onFailure(m_last_event);
return stopped;
}
}
return true;
}
void
onVehicleManeuver(const IMC::VehicleState* vs)
{
if (!execMode() || pendingReply())
return;
if (vs->flags & IMC::VehicleState::VFLG_MANEUVER_DONE)
{
if (m_plan->isDone())
{
vehicleRequest(IMC::VehicleCommand::VC_STOP_MANEUVER);
std::string comp = DTR("plan completed");
onSuccess(comp, false);
m_pcs.last_outcome = IMC::PlanControlState::LPO_SUCCESS;
m_reply.plan_id = m_spec.plan_id;
changeMode(IMC::PlanControlState::PCS_READY, comp);
}
else
{
IMC::PlanManeuver* pman = m_plan->loadNextManeuver();
startManeuver(pman);
}
}
else
{
m_pcs.man_eta = vs->maneuver_eta;
}
}
//! Stop current plan being executed
//! @param[in] plan_startup true if a plan will start right after this stop
//! @return false if a plan is still running after this
bool
stopPlan(bool plan_startup = false)
{
if (initMode() || execMode())
{
if (!plan_startup)
{
// stop maneuver only if we are not executing a plan afterwards
vehicleRequest(IMC::VehicleCommand::VC_STOP_MANEUVER);
m_reply.plan_id = m_spec.plan_id;
m_pcs.last_outcome = IMC::PlanControlState::LPO_FAILURE;
changeMode(IMC::PlanControlState::PCS_READY, DTR("plan stopped"));
}
else
{
m_pcs.last_outcome = IMC::PlanControlState::LPO_FAILURE;
debug("switching to new plan");
return false;
}
}
else
{
if (!plan_startup)
{
onFailure(DTR("no plan is running, request ignored"));
m_reply.plan_id = "";
}
}
return true;
}
void
consume(const IMC::VehicleCommand* vc)
{
if (vc->type == IMC::VehicleCommand::VC_REQUEST)
return;
if ((vc->getDestination() != getSystemId()) ||
(vc->getDestinationEntity() != getEntityId()) ||
(m_vreq_ctr != vc->request_id))
return;
if (!pendingReply())
return;
m_vc_reply_deadline = -1;
bool error = vc->type == IMC::VehicleCommand::VC_FAILURE;
// Ignore failure if it failed to stop calibration
if (error && (vc->command == IMC::VehicleCommand::VC_STOP_CALIBRATION))
{
debug("%s", vc->info.c_str());
error = false;
}
if (initMode() || execMode())
{
if (error)
changeMode(IMC::PlanControlState::PCS_READY, vc->info, false);
return;
}
}
//! Report progress
void
reportProgress(void)
{
// Must be executing or calibrating to be able to compute progress
if (m_plan == NULL || (!execMode() && !initMode()))
return;
m_pcs.plan_progress = m_plan->updateProgress(&m_mcs);
m_pcs.plan_eta = (int32_t)m_plan->getETA();
}
//! Change the current plan control state mode
//! @param[in] s plan control state to switch to
//! @param[in] event_desc description of the event that motivated the change
//! @param[in] nid id of the maneuver if any
//! @param[in] maneuver pointer to maneuver message
//! @param[in] print true if the messages should be printed to output
void
changeMode(IMC::PlanControlState::StateEnum s, const std::string& event_desc,
const std::string& nid, const IMC::Message* maneuver, bool print = true)
{
double now = Clock::getSinceEpoch();
if (print)
war("%s", event_desc.c_str());
m_last_event = event_desc;
if (s != m_pcs.state)
{
debug(DTR("now in %s state"), DTR(c_state_desc[s]));
bool was_in_plan = initMode() || execMode();
m_pcs.state = s;
bool is_in_plan = initMode() || execMode();
if (was_in_plan && !is_in_plan)
{
m_plan->planStopped();
changeLog("");
}
}
if (maneuver)
{
m_pcs.man_id = nid;
m_pcs.man_type = maneuver->getId();
}
else
{
m_pcs.man_id.clear();
m_pcs.man_type = 0xFFFF;
}
m_pcs.setTimeStamp(now);
dispatch(m_pcs, DF_KEEP_TIME);
}
//! Get current plan
void
getPlan(void)
{
if (!initMode() && !execMode())
{
onFailure(DTR("no plan is running"));
return;
}
m_reply.arg.set(m_spec);
m_reply.plan_id = m_spec.plan_id;
onSuccess();
}
//! Load a plan into the vehicle
//! @param[in] plan_id name of the plan
//! @param[in] arg argument which may either be a maneuver or a plan specification
//! @param[in] plan_startup true if a plan will start right after
//! @return true if plan is successfully loaded
bool
loadPlan(const std::string& plan_id, const IMC::Message* arg,
bool plan_startup = false)
{
if ((initMode() && !plan_startup) || execMode())
{
onFailure(DTR("cannot load plan now"));
return false;
}
std::string info;
if (!parseArg(plan_id, arg, info))
{
changeMode(IMC::PlanControlState::PCS_READY,
DTR("plan load failed: ") + info);
return false;
}
IMC::PlanStatistics ps;
if (!parsePlan(plan_startup, ps))
{
changeMode(IMC::PlanControlState::PCS_READY,
DTR("plan parse failed: ") + m_reply.info);
return false;
}
// reply with statistics
m_reply.arg.set(ps);
m_reply.plan_id = m_spec.plan_id;
m_pcs.plan_id = m_spec.plan_id;
onSuccess(DTR("plan loaded"), false);
return true;
}
void
onVehicleError(const IMC::VehicleState* vs)
{
std::string err_ents = DTR("vehicle errors: ") + vs->error_ents;
std::string edesc = vs->last_error_time < 0 ? err_ents : vs->last_error;
if (execMode())
{
onFailure(edesc);
m_reply.plan_id = m_spec.plan_id;
}
// there are new error entities
if (edesc != m_last_event)
{
if (initMode())
{
onFailure(edesc);
m_reply.plan_id = m_spec.plan_id;
}
changeMode(IMC::PlanControlState::PCS_BLOCKED, edesc, false);
}
}
int main(){
pid_t pid, sid;
int list_s; /* listening socket */
int conn_s; /* connection socket */
short int port = 1995; /* port number */
struct sockaddr_in servaddr; /* socket address structure */
int i;
uint8_t numClients = 0;
threadInfo* info;
/* Fork off the parent process */
pid = fork();
if(pid < 0) {
exit(EXIT_FAILURE);
}
/* If we got a good PID, then we can exit the parent process */
if(pid > 0){
exit(EXIT_SUCCESS);
}
umask(0);
printf("Daemon started. Writing all further notices to daemon log: /var/log/daemon.log\n");
// Enable Signal Handler
signal(SIGTERM, catch_term);
/* Open Log File Here */
openlog("SEASSERVER",LOG_PID,LOG_DAEMON);
syslog(LOG_DAEMON|LOG_INFO,"Daemon Started.");
/* Open Config File Here */
if(!readConfig()){
syslog(LOG_DAEMON|LOG_ERR,"Unable to Read Configuration File. Daemon Terminated.\n");
exit(EXIT_FAILURE);
}
/* Create a new SID for the child process */
sid = setsid();
if(sid < 0){
syslog(LOG_DAEMON|LOG_ERR,"Unable to create a new SID for child process. Daemon Terminated.");
exit(EXIT_FAILURE);
}
/* Change the current working directory */
if((chdir("/")) < 0){
syslog(LOG_DAEMON|LOG_ERR,"Unable to switch working directory. Daemon Terminated.");
exit(EXIT_FAILURE);
}
close(STDIN_FILENO);
close(STDOUT_FILENO);
close(STDERR_FILENO);
/* Setup TCP/IP Socket */
if((list_s = socket(AF_INET, SOCK_STREAM, 0)) < 0){
syslog(LOG_DAEMON|LOG_ERR,"Unable to create socket. Daemon Terminated.");
exit(EXIT_FAILURE);
}
memset(&servaddr, 0, sizeof(servaddr));
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
servaddr.sin_port = htons(port);
#ifdef DEBUG
syslog(LOG_DAEMON|LOG_INFO,"Opening Spectrometers.");
#endif
// Power Cycle GPIO for USB Hub to make sure USB ports initialized correctly
//system("echo 0 > /sys/class/gpio/gpio168/value");
//sleep(3);
//system("echo 1 > /sys/class/gpio/gpio168/value");
//sleep(3);
// Connect the USB Spectrometers
char *serialNumbers[NUM_SPECS] = {getSerialNumber(0),getSerialNumber(1)};
if(connectSpectrometers(serialNumbers) == CONNECT_ERR)
{
syslog(LOG_DAEMON|LOG_ERR,"Spectrometers could not be opened. Daemon Exiting");
exit(EXIT_FAILURE);
}
#ifdef DEBUG
syslog(LOG_DAEMON|LOG_INFO,"Spectrometers Opened.");
#endif
applyConfig();
logConfig();
// Start LON Dispatch
syslog(LOG_DAEMON|LOG_INFO,"Starting LON Connection.");
if(startDispatch(LONPORT) == -1){
syslog(LOG_DAEMON|LOG_ERR,"LON Not Connected! Check serial port.");
exit(EXIT_FAILURE);
} else {
syslog(LOG_DAEMON|LOG_INFO,"LON Connection Started.");
}
// Init Bench Config and Power Management GPIOs (See SEASPeriperalCommands.c)
readBenchConfig();
initPeripherals();
// Start CTD Sink
CTDSink* ctdSink = CTDSink::Instance();
/* Bind our socket addresss to the
listening socket, and call listen() */
if ( bind(list_s, (struct sockaddr *) &servaddr, sizeof(servaddr)) < 0 ) {
syslog(LOG_DAEMON|LOG_ERR,"Unable to bind socket. Daemon Terminated.");
exit(EXIT_FAILURE);
}
if ( listen(list_s, NUM_THREADS) < 0 ) {
syslog(LOG_DAEMON|LOG_ERR,"Unable to listen on socket. Daemon Terminated.");
exit(EXIT_FAILURE);
}
for(i=0; i < NUM_THREADS; i++)
thread_bin_available[i] = AVAILABLE;
execMode();
while(keep_going){
syslog(LOG_DAEMON|LOG_INFO,"Listening for connection on port %i", port);
/* Wait for TCP/IP Connection */
conn_s = accept(list_s, NULL, NULL);
if ( conn_s < 0 ) {
syslog(LOG_DAEMON|LOG_ERR,"Unable to call accept() on socket.");
break;
}
/* Spawn a POSIX Server Thread to Handle Connected Socket */
for(i=0; i < NUM_THREADS; i++){
if(thread_bin_available[i]){
thread_bin_available[i] = UNAVAILABLE;
syslog(LOG_DAEMON|LOG_INFO,"Handling new connection on port %i",port);
numClients++;
info = (threadInfo*)malloc(sizeof(threadInfo));
info->socket_connection = conn_s;
info->thread_bin_index = i;
pthread_create(&thread_bin[i],NULL,handleConnection, (void*)info);
break;
}
}
if(i > NUM_THREADS){
syslog(LOG_DAEMON|LOG_ERR,"Unable to create thread to handle connection. Continuing...");
}
}
if(disconnectSpectrometers() == CONNECT_OK)
syslog(LOG_DAEMON|LOG_INFO,"Spectrometers Successfully Disconnected");
else
syslog(LOG_DAEMON|LOG_ERR,"Unable to Disconnect Spectrometers");
syslog(LOG_DAEMON|LOG_INFO,"Daemon Exited Politely.");
exit(EXIT_SUCCESS);
}
//! Load a plan into the vehicle
//! @param[in] plan_id name of the plan
//! @param[in] arg argument which may either be a maneuver or a plan specification
//! @param[in] plan_startup true if a plan will start right after
//! @return true if plan is successfully loaded
bool
loadPlan(const std::string& plan_id, const IMC::Message* arg,
bool plan_startup = false)
{
if ((initMode() && !plan_startup) || execMode())
{
onFailure(DTR("cannot load plan now"));
return false;
}
if (arg)
{
if (arg->getId() == DUNE_IMC_PLANSPECIFICATION)
{
const IMC::PlanSpecification* given_plan = static_cast<const IMC::PlanSpecification*>(arg);
m_spec = *given_plan;
m_spec.setSourceEntity(getEntityId());
}
else
{
// Quick plan
IMC::PlanManeuver spec_man;
const IMC::Maneuver* man = static_cast<const IMC::Maneuver*>(arg);
if (man)
{
spec_man.maneuver_id = arg->getName();
spec_man.data.set(*man);
m_spec.clear();
m_spec.maneuvers.setParent(&m_spec);
m_spec.plan_id = plan_id;
m_spec.start_man_id = arg->getName();
m_spec.maneuvers.push_back(spec_man);
}
else
{
changeMode(IMC::PlanControlState::PCS_READY,
DTR("plan load failed: undefined maneuver or plan"));
return false;
}
}
}
else
{
// Search DB
m_spec.clear();
if (!lookForPlan(plan_id, m_spec))
{
changeMode(IMC::PlanControlState::PCS_READY,
DTR("plan load failed: ") + m_reply.info);
return false;
}
}
if (!parsePlan(plan_startup))
{
changeMode(IMC::PlanControlState::PCS_READY,
DTR("plan validation failed: ") + m_reply.info);
return false;
}
m_pcs.plan_id = m_spec.plan_id;
if (plan_startup)
onSuccess(DTR("plan loaded"), false);
else
changeMode(IMC::PlanControlState::PCS_READY, DTR("plan loaded"));
return true;
}
