acs.c File Reference

#include "acs.h"

Include dependency graph for acs.c:

Go to the source code of this file.

Macros
#define	EVENT_COUNT   (int)(sizeof(trap)/sizeof(acs_trap))
	determines the total number of events More...
#define	TRANS_COUNT   (int)(sizeof(trans)/sizeof(acs_transition))
	TRANS_COUNT define the total number of legal transitions. More...

Functions
static void	trans_cleanup (ACS *acs)
	cleans up on transition More...
static void	update_recv (ACS *acs, int recv_byte)
	updates the recv buffer More...
static int	state_off (ACS *acs)
	returns the off state More...
static int	state_init (ACS *acs)
	returns the init state More...
static int	state_rdy (ACS *acs)
	returns the ready state More...
static int	state_rw (ACS *acs)
	return the reaction wheel state More...
static int	state_mtqr (ACS *acs)
	returns the magnetorquer state More...
static int	trap_rw_start (ACS *acs)
	function to start the the reaction wheels More...
static int	trap_rw_stop (ACS *acs)
	function to stop the reaction wheels More...
static int	trap_mtqr_start (ACS *acs)
	function to start the magnetorquer More...
static int	trap_mtqr_stop (ACS *acs)
	function to stop the magnetorquer More...
static int	trap_mtqr_dc (ACS *acs)
	function to change the PWM duty cycle for the magnetorquer More...
static int	trap_mtqr_dir (ACS *acs)
	function for function for changing the polarity of the magnetorquer More...
static int	trap_rw_stretch (ACS *acs)
	Changes how many steps are added in between each LUT step. More...
static int	trap_rw_control (ACS *acs)
	Closed loop using encoder, or brute forcing with open loop. More...
static int	trap_rw_skip (ACS *acs)
	Changes how many steps are skipped in the LUT when going to next step. More...
static int	trap_rw_scale (ACS *acs)
	Changes the scale factor, modifying LUT value by 0-100%. More...
static int	fsm_trap (ACS *acs)
	control falls here when a state change request is unrecognized More...
static acs_event	getNextEvent (ACS *acs)
	waits for events on the CAN bus and processes them More...
static int	acs_statemachine (ACS *acs)
	the ACS state machine More...
int	acsInit (ACS *acs)
	general ACS initialization More...
	THD_WORKING_AREA (wa_acsThread, WA_ACS_THD_SIZE)
	ACS thread and thread working area. More...
	THD_FUNCTION (acsThread, acs)

Variables
event_listener_t	el
const acs_trap	trap []
	trap function table More...
const acs_transition	trans []
	state transition table More...

Macro Definition Documentation

EVENT_COUNT

#define EVENT_COUNT   (int)(sizeof(trap)/sizeof(acs_trap))

determines the total number of events

TRANS_COUNT

#define TRANS_COUNT   (int)(sizeof(trans)/sizeof(acs_transition))

TRANS_COUNT define the total number of legal transitions.

Function Documentation

acs_statemachine()

static int acs_statemachine ( ACS *  acs )

static

the ACS state machine

Note

this is a critical section

                                     {
    int i;
    acs->cur_state = state_init(acs);
    update_recv(acs,ACS_STATE);
    
    while (!chThdShouldTerminateX() && acs->cur_state != ST_OFF) {
        acs->event = getNextEvent(acs);
    
        // *******critical section**********
        chSysLock();
        acs->can_buf.send[LAST_EVENT]=acs->event;
        chSysUnlock();
        // *******end critical section**********
        
        for (i = 0;i < TRANS_COUNT;++i) {
            if((acs->cur_state == trans[i].state)||(ST_ANY == trans[i].state)){
                if((acs->event == trans[i].event)||(EV_ANY == trans[i].event)){
                    acs->cur_state = (trans[i].fn)(acs);
                    update_recv(acs,ACS_STATE);
                }
            }
        }
    chThdSleepMilliseconds(500);
/*
    // this stuff is for debugging  
    chprintf(DEBUG_CHP, "motor stretch: %d\r\n", acs->motor.stretch);
    chprintf(DEBUG_CHP, "motor openLoop: %d\r\n", acs->motor.openLoop);
    chprintf(DEBUG_CHP, "motor skip: %d\r\n\n", acs->motor.skip);
    chprintf(DEBUG_CHP, "motor scale: %d\r\n\n", acs->motor.scale);
    chprintf(DEBUG_CHP, "motor samples: %d\r\n\n", acs->motor.samples[0]);
//*/
    }
    
    return EXIT_SUCCESS;
}

acsInit()

int acsInit

(

ACS *

acs

)

general ACS initialization

                            {
    (void)acs;
    mtqrInit(&acs->mtqr);
    return EXIT_SUCCESS;
}

fsm_trap()

static int fsm_trap ( ACS *  acs )

static

control falls here when a state change request is unrecognized

• Note

  this is a critical section

                             {
    int i,trap_status;

    for(i = 0;i < EVENT_COUNT;++i){
        if(acs->event == trap[i].event){
            if(trap[i].state == ST_ANY || acs->cur_state == trap[i].state){
                trap_status = (trap[i].fn)(acs);
                if(trap_status){
                    // *******critical section**********
                    chSysLock();
                    acs->can_buf.send[ERROR_CODE]=77;
                    chSysUnlock();
                    chThdSleepMilliseconds(500);
                    // *******end critical section**********
                }
            }
        }
    }

    return acs->cur_state;
}

getNextEvent()

static acs_event getNextEvent ( ACS *  acs )

static

waits for events on the CAN bus and processes them

Note

this is a critical section

Todo:

: we should seperate the trap from the state changes in the next version

                                       {
    acs_event event = EV_ANY;
    chEvtWaitAny(ALL_EVENTS);   
// ******critical section*******
    chSysLock();
    for(int i=0;i<CAN_BUF_SIZE;++i){
        acs->recv[i]=acs->can_buf.recv[i];
        acs->can_buf.recv[i]=0;
    }
    chSysUnlock();
// ******end critical section*******

    switch(acs->recv[MSG_TYPE]){
        case NOP:
            break;
        case CHG_STATE:
            event = acs->recv[ARG_BYTE];
      acs->data = acs->recv[ARG_BYTE+1];            
            break;
        case CALL_TRAP:
            //  event = EV_STATUS;
            // state command
            break;
        default:
            break;
    }

    if(event < EV_ANY || event >= EV_END){
        return (acs_event)acs->cur_state; 
    }

    return event;
}

state_init()

static int state_init ( ACS *  acs )

static

returns the init state

                               {
    (void)acs;
    return ST_INIT;
}

state_mtqr()

static int state_mtqr ( ACS *  acs )

static

returns the magnetorquer state

                               {
    trans_cleanup(acs);
    return ST_MTQR;
}

state_off()

static int state_off ( ACS *  acs )

static

returns the off state

                              {
    (void)acs;
    return ST_OFF;
}

state_rdy()

static int state_rdy ( ACS *  acs )

static

returns the ready state

                              {
    trans_cleanup(acs);
    return ST_RDY;
}

state_rw()

static int state_rw ( ACS *  acs )

static

return the reaction wheel state

                             {
    trans_cleanup(acs);
    bldcInit(&acs->motor);
    return ST_RW;
}

THD_FUNCTION()

THD_FUNCTION	(	acsThread	,
		acs
	)

                           {
  chRegSetThreadName("acsThread");
    chEvtRegister(&rpdo_event,&el,0);

    acs_statemachine(acs);
}

THD_WORKING_AREA()

THD_WORKING_AREA	(	wa_acsThread	,
		WA_ACS_THD_SIZE
	)

ACS thread and thread working area.

trans_cleanup()

static void trans_cleanup ( ACS *  acs )

static

cleans up on transition

Parameters

[in]

acs

pointer to the ACS object ACS caonnot be NULL

                                   {
    switch(acs->cur_state){
        case ST_OFF:
            
            break;
        case ST_INIT:

            break;
        case ST_RDY:

            break;

        case ST_RW:
            bldcExit(&acs->motor);
            break;

        case ST_MTQR:
            mtqrExit(&acs->mtqr);
            break;
        default:

            break;
    }
}

trap_mtqr_dc()

static int trap_mtqr_dc ( ACS *  acs )

static

function to change the PWM duty cycle for the magnetorquer

Note

this is a critical section

                                 {
    (void)acs;
    // *******critical section**********
    chSysLock();
    acs->can_buf.send[LAST_TRAP]=EV_MTQR_DC;
    chSysUnlock();
    // *******end critical section**********
    mtqrSetDC((acs->recv[ARG_BYTE+1]<<8) | acs->recv[ARG_BYTE+2]);
    return EXIT_SUCCESS;
}

trap_mtqr_dir()

static int trap_mtqr_dir ( ACS *  acs )

static

function for function for changing the polarity of the magnetorquer

Note

this is a critical section

                                  {
    (void)acs;
    // *******critical section**********
    chSysLock();
    acs->can_buf.send[LAST_TRAP]=EV_MTQR_STOP;
    chSysUnlock();
    // *******end critical section**********
    mtqrSetDir(acs->recv[ARG_BYTE+1]);
    return EXIT_SUCCESS;
}

trap_mtqr_start()

static int trap_mtqr_start ( ACS *  acs )

static

function to start the magnetorquer

Note

this is a critical section

                                    {
    // *******critical section**********
    chSysLock();
    acs->can_buf.send[LAST_TRAP]=EV_MTQR_START;
    chSysUnlock();
    // *******end critical section**********
    mtqrStart(&acs->mtqr);
    return EXIT_SUCCESS;
}

trap_mtqr_stop()

static int trap_mtqr_stop ( ACS *  acs )

static

function to stop the magnetorquer

Note

this is a critical section

                                   {
    (void)acs;
    // *******critical section**********
    chSysLock();
    acs->can_buf.send[LAST_TRAP]=EV_MTQR_STOP;
    chSysUnlock();
    // *******end critical section**********
    mtqrStop(&acs->mtqr);
    return EXIT_SUCCESS;
}

trap_rw_control()

static int trap_rw_control ( ACS *  acs )

static

Closed loop using encoder, or brute forcing with open loop.

Todo:

: have a better data system.

                                    {
  (void)acs;
  acs->motor.openLoop = (bool) acs->data;
  return EXIT_SUCCESS;
}

trap_rw_scale()

static int trap_rw_scale ( ACS *  acs )

static

Changes the scale factor, modifying LUT value by 0-100%.

Todo:

: have a better data system.

                                  {
  (void)acs;
  acs->motor.scale = acs->data;
  return EXIT_SUCCESS;
}

trap_rw_skip()

static int trap_rw_skip ( ACS *  acs )

static

Changes how many steps are skipped in the LUT when going to next step.

Todo:

: have a better data system.

                                 {
  (void)acs;
  acs->motor.skip = acs->data;
  return EXIT_SUCCESS;
}

trap_rw_start()

static int trap_rw_start ( ACS *  acs )

static

function to start the the reaction wheels

Note

this is a critical section

                                  {
    // *******critical section**********
    chSysLock();
    acs->can_buf.send[LAST_TRAP]=EV_RW_START;
    chSysUnlock();
    // *******end critical section**********
    bldcStart(&acs->motor);
    return EXIT_SUCCESS;
}

trap_rw_stop()

static int trap_rw_stop ( ACS *  acs )

static

function to stop the reaction wheels

Note

this is a critical section

                                 {
    // *******critical section**********
    chSysLock();
    acs->can_buf.send[LAST_TRAP]=EV_RW_STOP;
    chSysUnlock();
    // *******end critical section**********
    bldcStop(&acs->motor);
    return EXIT_SUCCESS;
}

trap_rw_stretch()

static int trap_rw_stretch ( ACS *  acs )

static

Changes how many steps are added in between each LUT step.

Todo:

: have a better data system.

                                    {
  (void)acs;
  acs->motor.stretch = acs->data;
  return EXIT_SUCCESS;
}

update_recv()

static void update_recv ( ACS *  acs, int  recv_byte  )

static

updates the recv buffer

Note

this is a critical section

                                               {
    switch(recv_byte){
        case ERROR_CODE:
            break;
        case ACS_STATE:
// *******critical section**********
            chSysLock();
            acs->can_buf.send[recv_byte]=acs->cur_state;
            chSysUnlock();
// *******end critical section**********
            break;
        default:
            break;
    }
}

Variable Documentation

el

event_listener_t el

trans

const acs_transition trans[]

Initial value:

= {
    {ST_INIT,   EV_RDY,     &state_rdy},
    {ST_INIT,   EV_OFF,     &state_off},
    {ST_RDY,        EV_RW,      &state_rw},
    {ST_RDY,        EV_MTQR,    &state_mtqr},
    {ST_RDY,        EV_OFF,     &state_off},
    {ST_RW,         EV_RDY,     &state_rdy},
    {ST_MTQR,   EV_RDY,     &state_rdy},
    {ST_ANY,        EV_ANY,     &fsm_trap}
}

state transition table

this state transistion table defines legal transitions and and what function is called upon a successful match in the table.

trap

const acs_trap trap[]

Initial value:

= {
    {ST_RW,     EV_RW_START,        &trap_rw_start},
    {ST_RW,     EV_RW_STOP,         &trap_rw_stop},
  {ST_RW,   EV_RW_STRETCH,  &trap_rw_stretch},
  {ST_RW,   EV_RW_CONTROL,  &trap_rw_control},
  {ST_RW,   EV_RW_SKIP,         &trap_rw_skip},
  {ST_RW,   EV_RW_SCALE,    &trap_rw_scale},
    {ST_MTQR,   EV_MTQR_START,  &trap_mtqr_start},
    {ST_MTQR,   EV_MTQR_STOP,       &trap_mtqr_stop},
    {ST_MTQR,   EV_MTQR_DC,         &trap_mtqr_dc},
    {ST_MTQR,   EV_MTQR_DIR,        &trap_mtqr_dir},
}

trap function table

this table defines the events allowed to be called in states, and the function that needs to be called on a successful match